add AutoWare2025 for AutoWare 2025 ros2 module.

src/ros2/AutoWare2025/rospilot.workspace

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<Workspace>
+    <Distribution path="/opt/ros/humble"/>
+    <DefaultBuildSystem value="2"/>
+</Workspace>

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/LICENSE

@@ -0,0 +1,13 @@
+Copyright 2022 LeiShen Intelligent
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/README_en.md

@@ -0,0 +1,218 @@
+# LSLIDAR_CX_V3.0.4_221228_ROS2
+
+## 1.Introduction
+​		LSLIDAR_CX_V3.0.4_221228_ROS2 is the lidar ros driver in linux environment, which is suitable for c16/c32 lidar(1212bytes ,version 2.6/2.8/3.0). The program has  tested under ubuntu18.04 ros dashing , ubuntu18.04 ros eloquent, ubuntu 20.04 ros foxy , ubuntu 20.04 ros galactic and ubuntu 22.04 ros humble .
+
+## 2.Dependencies
+
+1.ros
+
+To run lidar driver in ROS environment, ROS related libraries need to be installed.
+
+**Ubuntu 18.04**: ros-dashing-desktop-full
+
+**Ubuntu 18.04**: ros-eloquent-desktop-full
+
+**Ubuntu 20.04**: ros-foxy-desktop-full
+
+**Ubuntu 20.04**: ros-galactic-desktop-full
+
+**Ubuntu 22.04**: ros-humble-desktop-full
+
+**Installation**: please refer to [http://wiki.ros.org](http://wiki.ros.org/)
+
+2.ros dependencies
+
+```bash
+# install
+sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions
+```
+
+3.other dependencies
+
+~~~bash
+sudo apt-get install libpcap-dev
+sudo apt-get install libboost${BOOST_VERSION}-dev   #Select the appropriate version
+~~~
+
+## 3.Compile & Run
+
+### 3.1 Compile
+
+~~~bash
+mkdir -p ~/lidar_ws/src
+~~~
+
+Copy the whole lidar ROS driver directory into ROS workspace, i.e "~/lidar_ws/src".
+
+~~~bash
+cd ~/lidar_ws
+colcon build
+source install/setup.bash
+~~~
+
+### 3.2 Run
+
+run with single c32 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c32.yaml when start  lidar driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c32_launch.py
+~~~
+
+run with double c32 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c32_1.yaml and  lslidar_c32_2.yaml  when start  lidar  driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c32_double_launch.py
+~~~
+
+
+
+run with single c16 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c16.yaml when start  lidar driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c16_launch.py
+~~~
+
+run with double c16 lidar(It is divided into 1212 bytes lidar and 1206 bytes lidar. Pay attention to modify lslidar_driver/params/lslidar_c16_1.yaml and  lslidar_c16_2.yaml  when start lidar  driver) :
+
+~~~bash
+ros2 launch lslidar_driver lslidar_c16_double_launch.py
+~~~
+
+
+
+## 4. Introduction to parameters
+
+The content of the lslidar_c32.yaml file is as follows, and the meaning of each parameter is shown in the notes.
+
+~~~bash
+/c32/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                       #lidar data packet length (bytes)，write 1212 or 1206 
+    device_ip: 192.168.1.200                #lidar ip
+    msop_port: 2368                         # Main data Stream Output Protocol packet port
+    difop_port: 2369                        # Device Information Output Protocol packet port
+    frame_id: laser_link                    # lidar point cloud coordinate system name
+    add_multicast: false                    # Whether to add multicast
+    group_ip: 224.1.1.2                     #multicast ip
+    use_gps_ts: true                        # Whether gps time synchronization
+    lidar_type: c32                         #c16:c16 lidar；c32:c32 lidar
+    c16_type: c16_2                         #c16_2:lidar with vertical angular resolution of 2 degrees，c16_1:lidar with vertical angular resolution of 1.33 degrees
+    c32_type: c32_2                         #c32_2:lidar with vertical angular resolution of 1 degrees ，c32_1:lidar with vertical angular resolution of 2 degrees
+    c32_fpga_type: 3                        #3:lidar with fpga version 2.7\2.8\3.0 ，2:lidar with fpga version 2.6
+    min_range: 0.3                          #Unit: m. The minimum value of the lidar blind area, points smaller than this value are filtered
+    max_range: 200.0                        #Unit: m. The maximum value of the lidar blind area, points smaller than this value are filtered
+    distance_unit: 0.4                      #lidar range resolution
+    angle_disable_min: 0                    #lidar clipping angle start value ，unit:0.01°
+    angle_disable_max: 0                    #lidar clipping angle end value ，unit:0.01°
+    scan_num: 16                            #laserscan line number
+    topic_name: lslidar_point_cloud         #point cloud topic name, can be modified
+    publish_scan: false                     #Whether to publish the scan
+    pcl_type: false                         #pointcloud type，false: xyzirt,true:xyzi
+    coordinate_opt: false                   #Default false. The zero degree angle of the lidar corresponds to the direction of the point cloud
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/gps.pcap                        #Uncomment to read the data from the pcap file, and add the comment to read the data from the lidar
+~~~
+
+### Multicast mode:
+
+- The host computer sets the lidar to enable multicast mode
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  add_multicast: true
+  group_ip: 224.1.1.2    //The multicast ip address set by the host computer
+  ~~~
+
+- Run the following command to add the computer to the group (replace enp2s0 in the command with the network card name of the user's computer, you can use ifconfig to view the network card name)
+
+  ~~~shell
+  ifconfig
+  sudo route add -net 224.0.0.0/4 dev enp2s0
+  ~~~
+
+
+
+### Offline pcap mode:
+
+- Copy the recorded pcap file to the lslidar_ros/lslidar_driver/pcap folder
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  // uncomment
+  pcap: xxx.pcap  // xxx.pcap is changed to the copied pcap file name
+  ~~~
+
+
+
+###  pcl point cloud type:
+
+- Modify the following parameters of the launch file
+
+  ~~~xml
+  pcl_type: false      # pointcloud type，false: xyzirt,true:xyzi
+  ~~~
+
+- The default false is the custom point cloud type, which references the definition in the file of
+
+  lslidar_driver/include/lslidar_driver.h
+
+  Change it to true, which is the own type of pcl:
+
+  ~~~c++
+  pcl::PointCloud<pcl::PointXYZI>
+  ~~~
+
+## FAQ
+
+Bug Report
+
+version : LSLIDAR_CX_ 1212BYTES_V3.0.1_220719 ROS2
+
+Modify:  original version
+
+Date    : 2022-07-19
+
+-------------------------------------------------------------------------------------------------------------------------
+
+
+
+Updated version: LSLIDAR_CX_V3.0.2_220809_ROS2
+
+Modify:  1.Compatible with 1212 bytes and 1206 bytes C16 / C32 lidar(version 3.0 )
+
+Date    : 2022-08-09
+
+-------------------------
+
+
+
+Updated version : LSLIDAR_CX_V3.0.3_221118_ROS2
+
+Modify:  1.Added support for ros2 humble/ros2 dashing / ros2 eloquent
+
+Date    : 2022-11-18
+
+-------------------------
+
+
+
+Updated version : LSLIDAR_CX_V3.0.4_221228_ROS2
+
+Modify:  1.Fixed the problem of point cloud layering caused by negative C32 lidar compensation angle.
+
+Date    : 2022-11-18
+
+
+
+Updated version : LSLIDAR_CX_V3.0.4_221228_ROS2
+
+Modify:  Reduce cpu usage, boost library to standard library,point time to relative time .
+
+Date    : 2023-03-14
+
+-------------
+
+
+

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_driver)
+
+set(CMAKE_BUILD_TYPE Release)    #RelWithDebInfo
+#add_compile_options(-std=c++11)
+# 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)
+#find_package(angles REQUIRED)
+find_package(PCL REQUIRED)
+find_package(lslidar_msgs REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+#find_package(Boost REQUIRED COMPONENTS thread)
+find_package(sensor_msgs REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(pcl_conversions REQUIRED)
+#find_package(diagnostic_updater REQUIRED)
+find_package(PkgConfig REQUIRED)
+
+include_directories(
+		include
+		${PCL_INCLUDE_DIRS}
+#		${Boost_INCLUDE_DIRS}
+)
+
+# c32 Node
+add_executable(lslidar_c32driver_node src/lslidar_c32driver_node.cpp src/lslidar_driver.cpp src/input.cpp)
+target_link_libraries(lslidar_c32driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES})   #${Boost_LIBRARIES}
+ament_target_dependencies(lslidar_c32driver_node rclcpp std_msgs lslidar_msgs sensor_msgs  pcl_conversions)
+
+# c16 Node
+add_executable(lslidar_c16driver_node src/lslidar_c16driver_node.cpp src/lslidar_driver.cpp src/input.cpp)
+target_link_libraries(lslidar_c16driver_node ${rclcpp_LIBRARIES} ${libpcap_LIBRARIES} ${Boost_LIBRARIES})   #${Boost_LIBRARIES}
+ament_target_dependencies(lslidar_c16driver_node rclcpp std_msgs lslidar_msgs sensor_msgs  pcl_conversions)
+
+install(DIRECTORY launch params rviz_cfg
+		DESTINATION share/${PROJECT_NAME})
+
+install(TARGETS
+		lslidar_c32driver_node lslidar_c16driver_node
+		DESTINATION lib/${PROJECT_NAME}
+		)
+
+ament_export_dependencies(rclcpp std_msgs lslidar_msgs sensor_msgs pcl_conversions)
+ament_export_include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
+
+ament_package()

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/input.h

@@ -0,0 +1,122 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#ifndef __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 <lslidar_msgs/msg/lslidar_point.hpp>
+#include <lslidar_msgs/msg/lslidar_scan.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>
+
+namespace lslidar_driver {
+    static uint16_t MSOP_DATA_PORT_NUMBER = 2368;   // lslidar default data port on PC
+    static uint16_t DIFOP_DATA_PORT_NUMBER = 2369;  // lslidar default difop 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 &pkt) = 0;
+
+
+    protected:
+        rclcpp::Node*  private_nh_;
+        uint16_t port_;
+        std::string devip_str_;
+        int cur_rpm_;
+        int return_mode_;
+        bool npkt_update_flag_;
+        bool add_multicast;
+        std::string group_ip;
+        int packet_size_;
+
+    };
+
+/** @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 &pkt);
+
+
+    private:
+        int sockfd_;
+        in_addr devip_;
+
+    };
+
+/** @brief lslidar input from PCAP dump file.
+   *
+   * Dump files can be grabbed by libpcap
+   */
+    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 = "");
+
+//        InputPCAP(rclcpp::Node* private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
+//                  std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0);
+
+        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/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/include/lslidar_driver/lslidar_driver.h

@@ -0,0 +1,313 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+
+#ifndef _LS_C16_DRIVER_H_
+#define _LS_C16_DRIVER_H_
+
+#define DEG_TO_RAD 0.017453293
+#define RAD_TO_DEG 57.29577951
+
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+#include <pcl/point_types.h>
+//#include <pcl_ros/impl/transforms.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+//#include <boost/shared_ptr.hpp>
+//#include <boost/thread.hpp>
+#include <thread>
+#include <memory>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+//#include <sensor_msgs/msg/laser_scan.h>
+#include "input.h"
+//#include <pcl_ros/point_cloud.h>
+//#include <sensor_msgs/msg/point_cloud.h>
+#include <std_msgs/msg/string.h>
+#include <lslidar_msgs/msg/lslidar_packet.hpp>
+#include <lslidar_msgs/msg/lslidar_point.hpp>
+#include <lslidar_msgs/msg/lslidar_scan.hpp>
+#include <lslidar_msgs/srv/lslidarcontrol.hpp>
+
+
+namespace lslidar_driver {
+//raw lslidar packet constants and structures
+    static const int SIZE_BLOCK = 100;
+    static const int RAW_SCAN_SIZE = 3;
+    static const int SCANS_PER_BLOCK = 32;
+    static const int BLOCK_DATA_SIZE = SCANS_PER_BLOCK * RAW_SCAN_SIZE;
+    static const double DISTANCE_RESOLUTION = 0.01; //meters
+    static const uint16_t UPPER_BANK = 0xeeff;
+
+// special defines for lslidarlidar support
+    static const int FIRING_TOFFSET = 32;
+//    static const int PACKET_SIZE = 1206;
+    static const int BLOCKS_PER_PACKET = 12;
+    static const int SCANS_PER_PACKET = SCANS_PER_BLOCK  * BLOCKS_PER_PACKET; //384
+    //unit:meter
+    static const double R1_ = 0.04319;
+    static const double R2_ = 0.0494;
+// Pre-compute the sine and cosine for the altitude angles.
+    //1.33
+    static const double c16_1_vertical_angle[16] = {-10, 0.665, -8.665, 2, -7.33, 3.33, -6, 4.665, -4.665, 6, -3.33,
+                                                    7.33, -2, 8.665, -0.665, 10};
+    //2°
+    static const double c16_2_vertical_angle[16] = {-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15};
+
+
+    // 0.33°
+    static const double c32_1_vertical_angle[32] = {-18, -1, -15, -0.66, -12, -0.33, -10, 0, -8, 0.33, -7, 0.66, -6, 1,
+                                                    -5, 1.33, -4, 1.66, -3.33, 2, -3, 3, -2.66, 4, -2.33, 6, -2, 8,
+                                                    -1.66, 11, -1.33, 14};
+    static const double c32_1_vertical_angle_26[32] = {-18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66, -3,
+                                                       -2.33, -2, -1.33, -1.66, -1, -0.66, 0, -0.33, 0.33, 0.66,
+                                                       1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14};
+    // 1°
+    static const double c32_2_vertical_angle[32] = {-16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8,
+                                                    8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15};
+
+    static const double c32_2_vertical_angle_26[32] = {-16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4,
+                                                       -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+                                                       15};
+    static const uint8_t adjust_angle_index[32] = {0, 16, 1, 17, 2, 18, 3, 19, 4, 20, 5, 21, 6, 22, 7, 23, 8, 24, 9,
+                                                   25, 10, 26, 11, 27, 12, 28, 13, 29, 14, 30, 15, 31};
+
+
+    union TwoBytes {
+        uint16_t distance;
+        uint8_t bytes[2];
+    };
+
+    struct RawBlock {
+        uint16_t header;
+        uint16_t rotation;  //0-35999
+        uint8_t data[BLOCK_DATA_SIZE];
+    };
+
+    struct RawPacket {
+        RawBlock blocks[BLOCKS_PER_PACKET];
+        uint32_t time_stamp;
+        uint8_t factory[2];
+    };
+
+    struct Firing {
+        uint16_t firing_azimuth[BLOCKS_PER_PACKET];
+        int azimuth[SCANS_PER_PACKET];
+        double distance[SCANS_PER_PACKET];
+        double intensity[SCANS_PER_PACKET];
+    };
+
+/*    struct FiringC32 {
+        uint16_t firing_azimuth[BLOCKS_PER_PACKET];
+        int azimuth[SCANS_PER_PACKET];
+        double distance[SCANS_PER_PACKET];
+        double intensity[SCANS_PER_PACKET];
+    };*/
+
+    struct PointXYZIRT {
+        PCL_ADD_POINT4D
+        float intensity;
+        std::uint16_t ring;
+        float time;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW //make sure our new allocators are aligned
+    }EIGEN_ALIGN16; //enforce SSE padding for correct memory alignment
+
+    static std::string lidar_type;
+
+    class lslidarDriver : public rclcpp::Node {
+    public:
+        lslidarDriver();
+
+        lslidarDriver(const rclcpp::NodeOptions &options);
+
+        virtual ~lslidarDriver() {}
+
+        bool checkPacketValidity(const RawPacket *packet);
+
+        //check if a point is in the required range
+        bool isPointInRange(const double &distance);
+
+        bool loadParameters();
+
+        void initTimeStamp();
+
+        bool createRosIO();
+
+        void publishPointcloud();
+
+        void publishScan();
+
+
+        bool lslidarControl(lslidar_msgs::srv::Lslidarcontrol::Request &req,
+                               lslidar_msgs::srv::Lslidarcontrol::Response &res);
+
+        bool SendPacketTolidar(bool power_switch);
+
+        virtual void difopPoll() = 0;
+
+        virtual bool poll() = 0;
+
+        virtual bool initialize() = 0;
+
+        virtual void decodePacket(const RawPacket *packet) = 0;
+
+
+    public:
+        int socket_id;
+        int msop_udp_port;
+        int difop_udp_port;
+        int scan_num;
+        int point_num;
+        int angle_disable_min;
+        int angle_disable_max;
+        uint16_t last_azimuth;
+        uint16_t last_azimuth_tmp;
+        uint64_t packet_time_s;
+        uint64_t packet_time_ns;
+        int return_mode;
+        int c32_fpga_type;
+        int config_vert_num;
+
+        in_addr lidar_ip;
+        std::string lidar_ip_string;
+        std::string group_ip_string;
+        std::string frame_id;
+        std::string dump_file;
+        std::string pointcloud_topic;
+        std::string c32_type;
+        std::string c16_type;
+
+        bool use_gps_ts;
+        bool pcl_type;
+        bool publish_scan;
+        bool coordinate_opt;
+        bool is_first_sweep;
+        bool add_multicast;
+        bool config_vert;
+        double distance_unit;
+        double min_range;
+        double max_range;
+        double sweep_end_time;
+        double horizontal_angle_resolution;
+        double packet_rate;
+        //double packet_start_time;
+        double angle_base;
+        double cos_azimuth_table[36000];
+        double sin_azimuth_table[36000];
+
+
+        std::shared_ptr<Input> msop_input_;
+        std::shared_ptr<Input> difop_input_;
+        std::shared_ptr<std::thread> difop_thread_;
+
+        lslidar_msgs::msg::LslidarScan::UniquePtr sweep_data;
+        //ros::Publisher packet_pub;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub;
+        rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
+        rclcpp::Service<lslidar_msgs::srv::Lslidarcontrol>::SharedPtr lslidar_control;
+
+        unsigned char difop_data[1212];
+        unsigned char packetTimeStamp[10];
+        struct tm cur_time;
+        rclcpp::Time timeStamp;
+
+        int count;
+        bool is_update_gps_time;
+        double current_packet_time;
+        double last_packet_time;
+        double current_point_time=0.0;
+        double last_point_time=0.0;
+        rclcpp::Time timeStamp_bak;
+        int packet_size;
+
+
+    };
+
+
+    class lslidarC16Driver : public lslidarDriver {
+    public:
+        /**
+       * @brief lslidarDriver
+       * @param node          raw packet output topic
+       * @param private_nh    通过这个节点传参数
+       */
+        lslidarC16Driver();
+
+        virtual ~lslidarC16Driver();
+
+        virtual void difopPoll();
+
+        virtual bool poll(void);
+
+        virtual bool initialize();
+
+        virtual void decodePacket(const RawPacket *packet);
+
+
+    private:
+        Firing firings;
+        double c16_vertical_angle[16];
+        double c16_config_vertical_angle[16];
+        //double c16_scan_altitude[16];
+        double c16_cos_scan_altitude[16];
+        double c16_sin_scan_altitude[16];
+
+    };
+
+
+    class lslidarC32Driver : public lslidarDriver {
+    public:
+        lslidarC32Driver();
+
+        virtual ~lslidarC32Driver();
+
+        virtual void difopPoll();
+
+        virtual bool poll(void);
+
+        virtual bool initialize();
+
+        virtual void decodePacket(const RawPacket *packet);
+
+
+    private:
+        Firing firings;
+        double c32_vertical_angle[32];
+        double c32_config_vertical_angle[32];
+        double c32_config_tmp_angle[32];
+        // double c32_scan_altitude[32];
+        double c32_cos_scan_altitude[32];
+        double c32_sin_scan_altitude[32];
+        int adjust_angle[4];
+        bool get_difop;
+    };
+
+    typedef PointXYZIRT VPoint;
+    typedef pcl::PointCloud<VPoint> VPointcloud;
+
+}  // namespace lslidar_driver
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_driver::PointXYZIRT,
+                                  (float, x, x)
+                                          (float, y, y)
+                                          (float, z, z)
+                                          (float, intensity, intensity)
+                                          (std::uint16_t, ring, ring)
+                                          (float, time, time))
+
+#endif

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c16_double_launch.py

@@ -0,0 +1,85 @@
+#!/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
+import subprocess
+
+def generate_launch_description():
+    driver_dir_left = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c16_1.yaml')
+    driver_dir_right = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c16_2.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c16.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node_left = ""
+    driver_node_right = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         node_namespace='c16_left',
+                                         node_executable='lslidar_c16driver_node',
+                                         node_name='lslidar_driver_node',
+                                         output='screen',
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          node_namespace='c16_right',
+                                          node_executable='lslidar_c16driver_node',
+                                          node_name='lslidar_driver_node',
+                                          output='screen',
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c16',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         namespace='c16_left',
+                                         executable='lslidar_c16driver_node',
+                                         name='lslidar_driver_node',
+                                         output='screen',
+                                         emulate_tty=True,
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          namespace='c16_right',
+                                          executable='lslidar_c16driver_node',
+                                          name='lslidar_driver_node',
+                                          output='screen',
+                                          emulate_tty=True,
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c16',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node_left,
+        driver_node_right,
+        rviz_node
+    ])

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c16_launch.py

@@ -0,0 +1,63 @@
+#!/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
+import subprocess
+
+
+def generate_launch_description():
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c16.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c16.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    node_namespace='c16',
+                                    node_executable='lslidar_c16driver_node',
+                                    node_name='lslidar_driver_node',
+                                    output='screen',
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c16',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    namespace='c16',
+                                    executable='lslidar_c16driver_node',
+                                    name='lslidar_driver_node',
+                                    output='screen',
+                                    emulate_tty=True,
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c16',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node, rviz_node
+    ])

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c32_double_launch.py

@@ -0,0 +1,85 @@
+#!/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
+import subprocess
+
+def generate_launch_description():
+    driver_dir_left = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c32_1.yaml')
+    driver_dir_right = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c32_2.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c32.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node_left = ""
+    driver_node_right = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         node_namespace='c32_left',
+                                         node_executable='lslidar_c32driver_node',
+                                         node_name='lslidar_driver_node',
+                                         output='screen',
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          node_namespace='c32_right',
+                                          node_executable='lslidar_c32driver_node',
+                                          node_name='lslidar_driver_node',
+                                          output='screen',
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c32',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node_left = LifecycleNode(package='lslidar_driver',
+                                         namespace='c32_left',
+                                         executable='lslidar_c32driver_node',
+                                         name='lslidar_driver_node',
+                                         output='screen',
+                                         emulate_tty=True,
+                                         parameters=[driver_dir_left],
+                                         )
+
+        driver_node_right = LifecycleNode(package='lslidar_driver',
+                                          namespace='c32_right',
+                                          executable='lslidar_c32driver_node',
+                                          name='lslidar_driver_node',
+                                          output='screen',
+                                          emulate_tty=True,
+                                          parameters=[driver_dir_right],
+                                          )
+
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c32',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node_left,
+        driver_node_right,
+        rviz_node
+    ])

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/launch/lslidar_c32_launch.py

@@ -0,0 +1,63 @@
+#!/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
+import subprocess
+
+
+def generate_launch_description():
+    driver_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lslidar_c32.yaml')
+    rviz_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'rviz_cfg', 'lslidar_c32.rviz')
+
+    p = subprocess.Popen("echo $ROS_DISTRO", stdout=subprocess.PIPE, shell=True)
+    driver_node = ""
+    rviz_node = ""
+    ros_version = p.communicate()[0]
+    print(ros_version)
+    if ros_version == b'dashing\n' or ros_version == b'eloquent\n':
+        print("ROS VERSION: dashing/eloquent")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    node_namespace='c32',
+                                    node_executable='lslidar_c32driver_node',
+                                    node_name='lslidar_driver_node',
+                                    output='screen',
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            node_namespace='c32',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+    elif ros_version == b'foxy\n' or ros_version == b'galactic\n' or ros_version == b'humble\n':
+        print("ROS VERSION: foxy/galactic/humble")
+        driver_node = LifecycleNode(package='lslidar_driver',
+                                    namespace='c32',
+                                    executable='lslidar_c32driver_node',
+                                    name='lslidar_driver_node',
+                                    output='screen',
+                                    emulate_tty=True,
+                                    parameters=[driver_dir],
+                                    )
+        rviz_node = Node(
+            package='rviz2',
+            namespace='c32',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_dir],
+            output='screen')
+
+    else:
+        print("Please configure the ros environment")
+        exit()
+
+    return LaunchDescription([
+        driver_node, rviz_node
+    ])

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/package.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lslidar_driver</name>
+  <version>1.0.0</version>
+
+  <maintainer email="tongsky723@sina.com">tongsky</maintainer>
+  <author>Yutong</author>
+  <license>BSD</license>
+  <description> ROS device driver for C32 lidar </description>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>rclpy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>rslidar_input</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>lslidar_msgs</build_depend>
+  <build_depend>libpcap</build_depend>
+
+  <exec_depend>angles</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>libpcl-all</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>lslidar_msgs</exec_depend>
+  <exec_depend>libpcap</exec_depend>
+
+
+  <depend>diagnostic_updater</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c16.yaml

@@ -0,0 +1,27 @@
+/c16/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                      # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c16                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.25
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 8
+    horizontal_angle_resolution: 0.18
+    packet_rate: 840.0
+    topic_name: lslidar_point_cloud
+    publish_scan: true
+    pcl_type: false
+    coordinate_opt: false
+#    pcap: /home/ls/xxx.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c16_1.yaml

@@ -0,0 +1,27 @@
+/c16_left/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c16                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.25
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 840.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c16_2.yaml

@@ -0,0 +1,27 @@
+/c16_right/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c16                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.25
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 840.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c32.yaml

@@ -0,0 +1,27 @@
+/c32/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c32                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.4
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 1700.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/gps.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c32_1.yaml

@@ -0,0 +1,27 @@
+/c32_left/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                  # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c32                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.4
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 1700.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/params/lslidar_c32_2.yaml

@@ -0,0 +1,27 @@
+/c32_right/lslidar_driver_node:
+  ros__parameters:
+    packet_size: 1206                      # 雷达数据包长度(字节)，填1212/1206
+    device_ip: 192.168.1.200
+    msop_port: 2368
+    difop_port: 2369
+    frame_id: laser_link
+    add_multicast: false
+    group_ip: 224.1.1.2
+    use_gps_ts: false
+    lidar_type: c32                         #c16表示机械式16线雷达；c32表示机械式32线雷达
+    c16_type: c16_2                         #c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
+    c32_type: c32_2                         #c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
+    c32_fpga_type: 3                        #3表示32线fpga为2.7\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达
+    min_range: 0.3
+    max_range: 200.0
+    distance_unit: 0.4
+    angle_disable_min: 0
+    angle_disable_max: 0
+    scan_num: 15
+    horizontal_angle_resolution: 0.18
+    packet_rate: 1700.0
+    topic_name: lslidar_point_cloud
+    publish_scan: false
+    pcl_type: false
+    coordinate_opt: false
+    #pcap: /home/chris/Documents/leishen/1212bytes_c32/C32_220714_1801.pcap                        #pcap包路径，加载pcap包时打开此注释

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/rviz_cfg/lslidar_c16.rviz

@@ -0,0 +1,164 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.35121950507164
+    Tree Height: 787
+  - 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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 8
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /c16/lslidar_point_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: laser_link
+    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
+      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: 12.808707237243652
+      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.8897970914840698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.8403964042663574
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001620000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004bb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/rviz_cfg/lslidar_c32.rviz

@@ -0,0 +1,197 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.35121950507164
+    Tree Height: 787
+  - 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/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 8
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /c32/lslidar_point_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      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: ""
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /c32/scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: laser_link
+    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
+      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: 12.808707237243652
+      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.8897970914840698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.8403964042663574
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001620000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004bb0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/src/input.cpp

@@ -0,0 +1,292 @@
+#include "lslidar_driver/input.h"
+#include <cmath>
+
+extern volatile sig_atomic_t flag;
+namespace lslidar_driver {
+//    static const size_t packet_size = sizeof(lslidar_msgs::msg::LslidarPacket().data);
+////////////////////////////////////////////////////////////////////////
+// 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;
+        packet_size_=1212;
+//        private_nh->declare_parameter<std::string>("device_ip", "");
+//        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>("packet_size", 1206);
+
+        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("packet_size", packet_size_);
+		RCLCPP_INFO(private_nh->get_logger(), "line: %d,packet size: %d ",__LINE__,packet_size_);
+
+        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);
+    }
+
+
+////////////////////////////////////////////////////////////////////////
+// InputSocket class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @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_);
+        }
+
+        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);
+            //group.imr_interface.s_addr = inet_addr("192.168.1.102");
+
+            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_);
+    }
+
+/** @brief Get one lslidar packet. */
+    int InputSocket::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) {
+        struct pollfd fds[1];
+        fds[0].fd = sockfd_;
+        fds[0].events = POLLIN;
+        static const int POLL_TIMEOUT = 3000;  // one second (in msec)
+
+        sockaddr_in sender_address{};
+        socklen_t sender_address_len = sizeof(sender_address);
+        while (flag == 1)
+            // while (true)
+        {
+            // Receive packets that should now be available from the
+            // socket using a blocking read.
+            // 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 1;
+                }
+                if (retval == 0)  // poll() timeout?
+                {
+                    time_t curTime = time(NULL);
+                    struct tm *curTm = localtime(&curTime);
+                    char bufTime[30] = {0};
+                    sprintf(bufTime, "%d-%d-%d %d:%d:%d", curTm->tm_year + 1900, curTm->tm_mon + 1,
+                            curTm->tm_mday, curTm->tm_hour, curTm->tm_min, curTm->tm_sec);
+
+                    RCLCPP_WARN(private_nh_->get_logger(), "%s  lslidar poll() timeout, port: %d", bufTime,port_);
+                    /*
+                    char buffer_data[8] = "re-con";
+                    memset(&sender_address, 0, sender_address_len);          // initialize to zeros
+                    sender_address.sin_family = AF_INET;                     // host byte order
+                    sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER);  // port in network byte order, set any value
+                    sender_address.sin_addr.s_addr = devip_.s_addr;          // automatically fill in my IP
+                    sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len);
+                    */
+                    return 1;
+                }
+                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 1;
+                }
+            } while ((fds[0].revents & POLLIN) == 0);
+            ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size_, 0, (sockaddr * ) & sender_address,
+                                      &sender_address_len);
+
+            if (nbytes < 0) {
+                if (errno != EWOULDBLOCK) {
+                    perror("recvfail");
+                    RCLCPP_ERROR(private_nh_->get_logger(),"recvfail");
+                    return 1;
+                }
+            } else if ((size_t) nbytes == packet_size_) {
+                if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr) {
+                    RCLCPP_ERROR(private_nh_->get_logger(), "lidar IP parameter set error,please reset in launch file");
+                    continue;
+                } else
+                    break;  // done
+            }
+            RCLCPP_WARN(private_nh_->get_logger(), "[driver][socket] incomplete lslidar packet read: %d bytes", nbytes);
+        }
+        if (flag == 0) {
+            abort();
+        }
+
+        return 0;
+    }
+
+////////////////////////////////////////////////////////////////////////
+// InputPCAP class implementation
+////////////////////////////////////////////////////////////////////////
+
+/** @brief constructor
+   *
+   *  @param private_nh ROS private handle for calling node.
+   *  @param port UDP port number
+   *  @param packet_rate expected device packet frequency (Hz)
+   *  @param filename PCAP dump file name
+   */
+    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->declare_parameter<bool>("read_once", false);
+//        private_nh->declare_parameter<bool>("read_fast", false);
+//        private_nh->declare_parameter<double>("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_);
+
+        // Open the PCAP dump file
+        // ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
+        RCLCPP_WARN(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_ != "")  // using specific IP?
+        {
+            filter << "src host " << devip_str_ << " && ";
+        }
+        filter << "udp dst port " << port;
+        pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
+    }
+
+/** destructor */
+    InputPCAP::~InputPCAP(void) {
+        pcap_close(pcap_);
+    }
+
+/** @brief Get one lslidar packet. */
+    int InputPCAP::getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr &pkt) {
+        struct pcap_pkthdr *header;
+        const u_char *pkt_data;
+
+        // while (flag == 1)
+        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;
+
+                // Keep the reader from blowing through the file.
+                if (read_fast_ == false)
+                    packet_rate_.sleep();
+
+
+                memcpy(&pkt->data[0], pkt_data + 42, packet_size_);
+
+                if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 &&
+                    pkt->data[3] == 0x5A) {//difop
+                    int rpm = (pkt->data[8] << 8) | pkt->data[9];
+                    RCLCPP_WARN_ONCE(private_nh_->get_logger(),"lidar rpm: %d", rpm);
+                }
+                empty_ = false;
+                return 0;  // success
+            }
+
+            if (empty_)  // no data in file?
+            {
+                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(),"replaying lslidar dump file");
+
+            // I can't figure out how to rewind the file, because it
+            // starts with some kind of header.  So, close the file
+            // and reopen it with pcap.
+            pcap_close(pcap_);
+            pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
+            empty_ = true;  // maybe the file disappeared?
+        }                 // loop back and try again
+
+        if (flag == 0) {
+            abort();
+        }
+        return 0;
+    }
+}

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_c16driver_node.cpp

@@ -0,0 +1,42 @@
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/msg/string.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    signal(SIGINT, my_handler);
+    lidar_type = "c16";
+    printf("lslidar type: %s", lidar_type.c_str());
+
+    // start the driver
+    if ("c16" == lidar_type) {
+        auto node = std::make_shared<lslidar_driver::lslidarC16Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+
+    } else {
+        auto node = std::make_shared<lslidar_driver::lslidarC32Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+    }
+    return 0;
+}

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_c32driver_node.cpp

@@ -0,0 +1,40 @@
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/msg/string.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    signal(SIGINT, my_handler);
+    lidar_type = "c32";
+
+    // start the driver
+    if ("c16" == lidar_type) {
+        auto node = std::make_shared<lslidar_driver::lslidarC16Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+
+    } else {
+        auto node = std::make_shared<lslidar_driver::lslidarC32Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+    }
+    return 0;
+}

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver.cpp

@@ -0,0 +1,1508 @@
+/******************************************************************************
+ * This file is part of lslidar_cx driver.
+ *
+ * Copyright 2022 LeiShen Intelligent Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "lslidar_driver/lslidar_driver.h"
+
+namespace lslidar_driver {
+    lslidarDriver::lslidarDriver() : lslidarDriver(rclcpp::NodeOptions()) {
+        config_vert_num = 0;
+        return;
+    }
+
+    lslidarDriver::lslidarDriver(const rclcpp::NodeOptions &options) : Node("lslidar_node", options) {
+        config_vert_num = 0;
+        config_vert = true;
+        socket_id = -1;
+        last_azimuth = 0;
+        sweep_end_time = 0.0;
+        is_first_sweep = true;
+        return_mode = 1;
+        current_packet_time = 0.0;
+        last_packet_time = 0.0;
+        packet_size = 1212;
+        is_update_gps_time = false;
+        sweep_data.reset(new lslidar_msgs::msg::LslidarScan());
+
+        return;
+    }
+
+    bool lslidarDriver::checkPacketValidity(const lslidar_driver::RawPacket *packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->blocks[blk_idx].header != UPPER_BANK) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    bool lslidarDriver::isPointInRange(const double &distance) {
+        return (distance >= min_range && distance < max_range);
+    }
+
+    bool lslidarDriver::loadParameters() {
+        this->declare_parameter<std::string>("pcap", "");
+        this->declare_parameter<std::string>("frame_id", "laser_link");
+        this->declare_parameter<std::string>("lidar_type", "c16");
+        this->declare_parameter<std::string>("c16_type", "c16_2");
+        this->declare_parameter<std::string>("c32_type", "c32_2");
+        this->declare_parameter<int>("c32_fpga_type", 3);
+        this->declare_parameter<bool>("add_multicast", false);
+        this->declare_parameter<bool>("config_vert", true);
+        this->declare_parameter<std::string>("group_ip", "234.2.3.2");
+        this->declare_parameter<std::string>("device_ip", "192.168.1.200");
+        this->declare_parameter<int>("msop_port", 2368);
+        this->declare_parameter<int>("difop_port", 2369);
+        this->declare_parameter<int>("point_num", 2000);
+        this->declare_parameter<int>("scan_num", 8);
+        this->declare_parameter<double>("min_range", 0.3);
+        this->declare_parameter<double>("max_range", 150.0);
+        this->declare_parameter<double>("distance_unit", 0.25);
+        this->declare_parameter<double>("packet_rate", 840.0);
+        this->declare_parameter<double>("horizontal_angle_resolution", 0.18);
+        this->declare_parameter<int>("angle_disable_min", 0);
+        this->declare_parameter<int>("angle_disable_max", 0);
+        this->declare_parameter<int>("packet_size", 1212);
+        this->declare_parameter<bool>("use_gps_ts", false);
+        this->declare_parameter<bool>("pcl_type", false);
+        this->declare_parameter<bool>("publish_scan", false);
+        this->declare_parameter<bool>("coordinate_opt", false);
+        this->declare_parameter<std::string>("topic_name", "lslidar_point_cloud");
+
+        msop_udp_port = 0;
+        difop_udp_port = 0;
+        this->get_parameter("packet_size", packet_size);
+        this->get_parameter("pcap", dump_file);
+        this->get_parameter("frame_id", frame_id);
+        this->get_parameter("lidar_type", lidar_type);
+        this->get_parameter("c16_type", c16_type);
+        this->get_parameter("c32_type", c32_type);
+        this->get_parameter("c32_fpga_type", c32_fpga_type);
+        this->get_parameter("add_multicast", add_multicast);
+        this->get_parameter("config_vert", config_vert);
+        this->get_parameter("group_ip", group_ip_string);
+        this->get_parameter("device_ip", lidar_ip_string);
+        this->get_parameter("msop_port", msop_udp_port);
+        this->get_parameter("difop_port", difop_udp_port);
+        this->get_parameter("point_num", point_num);
+        this->get_parameter("scan_num", scan_num);
+        this->get_parameter("min_range", min_range);
+        this->get_parameter("max_range", max_range);
+        this->get_parameter("distance_unit", distance_unit);
+        this->get_parameter("packet_rate", packet_rate);
+        this->get_parameter("horizontal_angle_resolution", horizontal_angle_resolution);
+        this->get_parameter("angle_disable_min", angle_disable_min);
+        this->get_parameter("angle_disable_max", angle_disable_max);
+        this->get_parameter("use_gps_ts", use_gps_ts);
+        this->get_parameter("pcl_type", pcl_type);
+        this->get_parameter("publish_scan", publish_scan);
+        this->get_parameter("coordinate_opt", coordinate_opt);
+        this->get_parameter("topic_name", pointcloud_topic);
+
+        RCLCPP_INFO(this->get_logger(), "dump_file: %s", dump_file.c_str());
+        RCLCPP_INFO(this->get_logger(), "frame_id: %s", frame_id.c_str());
+        RCLCPP_INFO(this->get_logger(), "lidar_type: %s", lidar_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "c16_type: %s", c16_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "c32_type: %s", c32_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "device_ip: %s", lidar_ip_string.c_str());
+        RCLCPP_INFO(this->get_logger(), "c32_fpag_type: %d", c32_fpga_type);
+        RCLCPP_INFO(this->get_logger(), "config_vert: %d", config_vert);
+        RCLCPP_INFO(this->get_logger(), "msop_port: %d", msop_udp_port);
+        RCLCPP_INFO(this->get_logger(), "use_gps_ts: %d", use_gps_ts);
+        RCLCPP_INFO(this->get_logger(), "pcl_type: %d", pcl_type);
+        RCLCPP_INFO(this->get_logger(), "distance_unit: %f", distance_unit);
+        RCLCPP_INFO(this->get_logger(), "horizontal_angle_resolution: %f", horizontal_angle_resolution);
+        RCLCPP_INFO(this->get_logger(), "packet_rate: %f", packet_rate);
+        RCLCPP_INFO(this->get_logger(), "difop_udp_port: %d", difop_udp_port);
+        RCLCPP_INFO(this->get_logger(), "coordinate_opt: %d", coordinate_opt);
+        RCLCPP_INFO(this->get_logger(), "topic_name: %s", pointcloud_topic.c_str());
+        RCLCPP_INFO(this->get_logger(), "packet_size: %d", packet_size);
+
+        if (lidar_type == "c16") {
+            if (scan_num > 15) { scan_num = 15; }
+            else if (scan_num < 0) { scan_num = 0; }
+        } else {
+            if (scan_num > 31) { scan_num = 31; }
+            else if (scan_num < 0) { scan_num = 0; }
+        }
+
+        if (packet_size != 1206 && packet_size != 1212) {
+            packet_size = 1212;
+            RCLCPP_WARN(this->get_logger(), "packet_size input error, reassign to: %d", packet_size);
+        }
+
+
+        inet_aton(lidar_ip_string.c_str(), &lidar_ip);
+        if (add_multicast)
+            RCLCPP_INFO(this->get_logger(), "opening UDP socket: group_address %s", group_ip_string.c_str());
+        return true;
+    }
+
+    void lslidarDriver::initTimeStamp() {
+        for (int i = 0; i < 10; i++) {
+            this->packetTimeStamp[i] = 0;
+        }
+        this->packet_time_s = 0;
+        this->packet_time_ns = 0;
+        this->timeStamp = rclcpp::Time(0.0);
+        this->timeStamp_bak = rclcpp::Time(0.0);
+    }
+
+    bool lslidarDriver::createRosIO() {
+        pointcloud_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>(pointcloud_topic, 10);
+        scan_pub = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+//        lslidar_control =
+//                this->create_service<lslidar_msgs::srv::Lslidarcontrol>(
+//                        "lslidarcontrol",
+//                        std::bind(&lslidarC16Driver::lslidarControl, this));
+
+        if (dump_file != "") {
+            msop_input_.reset(new lslidar_driver::InputPCAP(this, msop_udp_port, packet_rate, dump_file));
+            difop_input_.reset(new lslidar_driver::InputPCAP(this, difop_udp_port, 1, dump_file));
+        } else {
+            msop_input_.reset(new lslidar_driver::InputSocket(this, msop_udp_port));
+            difop_input_.reset(new lslidar_driver::InputSocket(this, difop_udp_port));
+        }
+        difop_thread_ = std::shared_ptr<std::thread>(
+                new std::thread(std::bind(&lslidarDriver::difopPoll, this)));
+        return true;
+    }
+
+    void lslidarDriver::publishPointcloud() {
+        if (sweep_data->points.size() < 65 || !is_update_gps_time) {
+            return;
+        }
+/*        if (pointcloud_pub.getNumSubscribers() == 0) {
+            return;
+        }*/
+        if (pcl_type) {
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+            // pcl_conversions::toPCL(sweep_data->header).stamp;
+            point_cloud->header.frame_id = frame_id;
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(sweep_end_time * 1e6);
+            size_t j;
+            pcl::PointXYZI point;
+            if (sweep_data->points.size() > 0) {
+                for (j = 0; j < sweep_data->points.size(); ++j) {
+                    if ((sweep_data->points[j].azimuth > angle_disable_min) &&
+                        (sweep_data->points[j].azimuth < angle_disable_max)) {
+                        continue;
+                    }
+                    point.x = sweep_data->points[j].x;
+                    point.y = sweep_data->points[j].y;
+                    point.z = sweep_data->points[j].z;
+                    point.intensity = sweep_data->points[j].intensity;
+                    point_cloud->points.push_back(point);
+                    ++point_cloud->width;
+                }
+            }
+            sensor_msgs::msg::PointCloud2 pc_msg;
+            pcl::toROSMsg(*point_cloud, pc_msg);
+            pc_msg.header.stamp =  rclcpp::Time(sweep_end_time * 1e9);
+            pointcloud_pub->publish(pc_msg);
+        } else {
+            VPointcloud::Ptr point_cloud(new VPointcloud());
+            //pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+            // pcl_conversions::toPCL(sweep_data->header).stamp;
+            point_cloud->header.frame_id = frame_id;
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(sweep_end_time * 1e6);
+            size_t j;
+            VPoint point;
+            if (sweep_data->points.size() > 0) {
+                for (j = 0; j < sweep_data->points.size(); ++j) {
+                    if ((sweep_data->points[j].azimuth > angle_disable_min) &&
+                        (sweep_data->points[j].azimuth < angle_disable_max)) {
+                        continue;
+                    }
+                    point.x = sweep_data->points[j].x;
+                    point.y = sweep_data->points[j].y;
+                    point.z = sweep_data->points[j].z;
+                    point.intensity = sweep_data->points[j].intensity;
+                    point.ring = sweep_data->points[j].ring;
+                    point.time = sweep_data->points[j].time;
+
+                    point_cloud->points.push_back(point);
+                    ++point_cloud->width;
+                    current_point_time = point.time;
+                    //if(current_point_time - last_point_time<0.0)
+                    //    RCLCPP_WARN(this->get_logger(),"timestamp is rolled back! current point time: %.12f  last point time: %.12f",current_point_time,last_point_time);
+                    //last_point_time = current_point_time;
+                }
+            }
+            sensor_msgs::msg::PointCloud2 pc_msg;
+            pcl::toROSMsg(*point_cloud, pc_msg);
+            pc_msg.header.stamp =  rclcpp::Time(sweep_end_time * 1e9);
+            pointcloud_pub->publish(pc_msg);
+        }
+        return;
+    }
+
+    void lslidarDriver::publishScan() {
+        if (sweep_data->points.size() < 65 || !is_update_gps_time) {
+            return;
+        }
+        sensor_msgs::msg::LaserScan::UniquePtr scan_msg(new sensor_msgs::msg::LaserScan());
+        scan_msg->header.frame_id = frame_id;
+        int layer_num_local = scan_num;
+        RCLCPP_INFO_ONCE(this->get_logger(), "default channel is %d", layer_num_local);
+        scan_msg->header.stamp = rclcpp::Time(sweep_end_time * 1e9);
+
+        scan_msg->angle_min = 0.0;
+        scan_msg->angle_max = 2.0 * M_PI;
+        scan_msg->angle_increment = horizontal_angle_resolution * DEG_TO_RAD;
+        scan_msg->range_min = min_range;
+        scan_msg->range_max = max_range;
+
+        uint point_size = ceil((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
+        scan_msg->ranges.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        scan_msg->intensities.assign(point_size, std::numeric_limits<float>::quiet_NaN());
+        if (coordinate_opt) {
+            for (size_t j = 0; j < sweep_data->points.size(); ++j) {
+                if (layer_num_local == sweep_data->points[j].ring) {
+                    float horizontal_angle = sweep_data->points[j].azimuth * 0.01 * DEG_TO_RAD;
+                    uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                    point_index = (point_index <= point_size) ? point_index : (point_index % point_size);
+                    scan_msg->ranges[point_size - point_index - 1] = sweep_data->points[j].distance;
+                    scan_msg->intensities[point_size - point_index - 1] = sweep_data->points[j].intensity;
+                }
+            }
+        } else {
+            for (size_t j = 0; j < sweep_data->points.size(); ++j) {
+                if (layer_num_local == sweep_data->points[j].ring) {
+/*                    float h_angle =
+                            (sweep_data->points[j].azimuth + 9000.0) < 36000.0 ? sweep_data->points[j].azimuth + 9000.0
+                                                                               : sweep_data->points[j].azimuth - 27000.0;
+                    if(h_angle < 18000.0){
+                        h_angle = 18000.0 - h_angle;
+                    }else{
+                        h_angle = 36000.0 - (h_angle - 18000.0);
+                    }*/
+                    float h_angle = (45000.0 - sweep_data->points[j].azimuth) < 36000.0 ? 45000.0 -
+                                                                                          sweep_data->points[j].azimuth
+                                                                                        :
+                                    9000.0 - sweep_data->points[j].azimuth;
+                    // ROS_INFO("a=%f,b=%f",sweep_data->points[j].azimuth,h_angle);
+                    float horizontal_angle = h_angle * 0.01 * DEG_TO_RAD;
+                    uint point_index = (int) ((horizontal_angle - scan_msg->angle_min) / scan_msg->angle_increment);
+                    point_index = (point_index <= point_size) ? point_index : (point_index % point_size);
+                    scan_msg->ranges[point_index] = sweep_data->points[j].distance;
+                    scan_msg->intensities[point_index] = sweep_data->points[j].intensity;
+                }
+            }
+        }
+        scan_pub->publish(std::move(scan_msg));
+    }
+
+    bool lslidarDriver::lslidarControl(lslidar_msgs::srv::Lslidarcontrol::Request &req,
+                                       lslidar_msgs::srv::Lslidarcontrol::Response &res) {
+        RCLCPP_INFO(this->get_logger(), "--------------------------");
+        // sleep(1);
+        lslidar_msgs::msg::LslidarPacket::UniquePtr packet0(new lslidar_msgs::msg::LslidarPacket());
+        packet0->data[0] = 0x00;
+        packet0->data[1] = 0x00;
+        int rc_msop = -1;
+
+        if (req.lasercontrol == 1) {
+
+            if ((rc_msop = msop_input_->getPacket(packet0)) == 0) {
+                res.status = 1;
+                RCLCPP_WARN(this->get_logger(), "receive cmd: %d,already power on status", req.lasercontrol);
+                return true;
+            }
+            RCLCPP_WARN(this->get_logger(), "receive cmd: %d,power on", req.lasercontrol);
+            SendPacketTolidar(true);
+            double time1 = get_clock()->now().seconds();
+
+            do {
+                rc_msop = msop_input_->getPacket(packet0);
+                double time2 = get_clock()->now().seconds();
+                if ((time2 - time1) > 20) {
+                    res.status = 0;
+                    RCLCPP_WARN(this->get_logger(), "lidar connect error");
+                    return true;
+                }
+            } while ((rc_msop != 0) && (packet0->data[0] != 0xff) && (packet0->data[1] != 0xee));
+            sleep(5);
+            res.status = 1;
+        } else if (req.lasercontrol == 0) {
+            RCLCPP_WARN(this->get_logger(), "receive cmd: %d,power off", req.lasercontrol);
+            SendPacketTolidar(false);
+            res.status = 1;
+        } else {
+            RCLCPP_WARN(this->get_logger(), "cmd error");
+            res.status = 0;
+        }
+
+        return true;
+    }
+
+    bool lslidarDriver::SendPacketTolidar(bool power_switch) {
+        int socketid;
+        unsigned char config_data[1206];
+        //int data_port = difop_data[24] * 256 + difop_data[25];
+        mempcpy(config_data, difop_data, 1206);
+        config_data[0] = 0xAA;
+        config_data[1] = 0x00;
+        config_data[2] = 0xFF;
+        config_data[3] = 0x11;
+        config_data[4] = 0x22;
+        config_data[5] = 0x22;
+        config_data[6] = 0xAA;
+        config_data[7] = 0xAA;
+        config_data[8] = 0x02;
+        config_data[9] = 0x58;
+        if (power_switch) {
+            config_data[45] = 0x00;
+        } else {
+            config_data[45] = 0x01;
+        }
+
+        sockaddr_in addrSrv;
+        socketid = socket(2, 2, 0);
+        addrSrv.sin_addr.s_addr = inet_addr(lidar_ip_string.c_str());
+        addrSrv.sin_family = AF_INET;
+        addrSrv.sin_port = htons(2368);
+        sendto(socketid, (const char *) config_data, 1206, 0, (struct sockaddr *) &addrSrv, sizeof(addrSrv));
+        return 0;
+
+    }
+
+
+    lslidarC16Driver::lslidarC16Driver() : lslidarDriver() {
+//        this->initialize();
+        return;
+    }
+
+    lslidarC16Driver::~lslidarC16Driver() {
+        if (difop_thread_ != nullptr) {
+//            difop_thread_->interrupt();
+            difop_thread_->join();
+        }
+    }
+
+    bool lslidarC16Driver::initialize() {
+        RCLCPP_INFO(this->get_logger(), "lslidarC16Driver::initialize()");
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot load all required ROS parameters.");
+            return false;
+        }
+        if (c16_type == "c16_2") {
+            for (int j = 0; j < 16; ++j) {
+                c16_vertical_angle[j] = c16_2_vertical_angle[j];
+                c16_config_vertical_angle[j] = c16_2_vertical_angle[j];
+                c16_sin_scan_altitude[j] = sin(c16_vertical_angle[j] * DEG_TO_RAD);
+                c16_cos_scan_altitude[j] = cos(c16_vertical_angle[j] * DEG_TO_RAD);
+            }
+        } else {
+            for (int j = 0; j < 16; ++j) {
+                c16_vertical_angle[j] = c16_1_vertical_angle[j];
+                c16_config_vertical_angle[j] = c16_1_vertical_angle[j];
+                c16_sin_scan_altitude[j] = sin(c16_vertical_angle[j] * DEG_TO_RAD);
+                c16_cos_scan_altitude[j] = cos(c16_vertical_angle[j] * DEG_TO_RAD);
+            }
+        }
+
+
+        if (!createRosIO()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot create all ROS IO.");
+            return false;
+        }
+
+        // create the sin and cos table for different azimuth values
+        for (int j = 0; j < 36000; ++j) {
+            sin_azimuth_table[j] = sin(j * 0.01 * DEG_TO_RAD);
+            cos_azimuth_table[j] = cos(j * 0.01 * DEG_TO_RAD);
+        }
+       // angle_base = M_PI * 2 / point_num;
+        return true;
+    }
+
+/*    bool lslidarC16Driver::checkPacketValidity(const RawPacket *packet) {
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            if (packet->blocks[blk_idx].header != UPPER_BANK) {
+                return false;
+            }
+        }
+        return true;
+    }*/
+
+
+    void lslidarC16Driver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_msgs::msg::LslidarPacket::UniquePtr difop_packet_ptr(new lslidar_msgs::msg::LslidarPacket);
+        while (rclcpp::ok()) {
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                count++;
+                if (count == 2) { is_update_gps_time = true; }
+                for (int i = 0; i < packet_size; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+
+                int version_data = difop_packet_ptr->data[1202];
+                if (config_vert) {
+                    if (2 == version_data) {
+                        for (int j = 0; j < 16; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[234 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[234 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c16_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+                            if (fabs(c16_vertical_angle[j] - c16_config_vertical_angle[j]) > 1.5) {
+                                //c16_config_vertical_angle[j] = c16_vertical_angle[j];
+                                config_vert_num++;
+                            }
+
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 16; ++k) {
+                                c16_sin_scan_altitude[k] = sin(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                                c16_cos_scan_altitude[k] = cos(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+
+                    } else {
+                        for (int j = 0; j < 16; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[245 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[245 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c16_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+                            if (fabs(c16_vertical_angle[j] - c16_config_vertical_angle[j]) > 1.5) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 16; ++k) {
+                                c16_sin_scan_altitude[k] = sin(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                                c16_cos_scan_altitude[k] = cos(c16_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                    }
+                    config_vert = false;
+                }
+                if (packet_size == 1206) {
+                    if (2 == version_data) {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[41];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[40];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[39];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[38];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[37];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[36];
+                    } else {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[57];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[56];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[55];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[54];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[53];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[52];
+                    }
+                }
+
+            } else if (rc < 0) {
+                return;
+            }
+//            rclcpp::spinOnce();
+
+        }
+    }
+
+
+    void lslidarC16Driver::decodePacket(const RawPacket *packet) {
+        //couputer azimuth angle for each firing
+        //even numbers
+        for (size_t b_idx = 0; b_idx < BLOCKS_PER_PACKET; ++b_idx) {
+            firings.firing_azimuth[b_idx] = packet->blocks[b_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD;
+        }
+
+        // computer distance ,intensity
+        //12 blocks
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            const RawBlock &raw_block = packet->blocks[blk_idx];
+
+            int32_t azimuth_diff = 0;
+            if (1 == return_mode) {
+                if (blk_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 1] - firings.firing_azimuth[blk_idx];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 1];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                }
+            } else {
+                if (blk_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 2] - firings.firing_azimuth[blk_idx];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 2];
+                    azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+                }
+
+            }
+            for (size_t scan_idx = 0; scan_idx < SCANS_PER_BLOCK; ++scan_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_idx;
+                //azimuth
+                firings.azimuth[blk_idx * 32 + scan_idx] =
+                        firings.firing_azimuth[blk_idx] + scan_idx * azimuth_diff / FIRING_TOFFSET;
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.azimuth[blk_idx * 32 + scan_idx] % 36000;
+                // distance
+                firings.distance[blk_idx * 32 + scan_idx] =
+                        static_cast<double>(raw_block.data[byte_idx] + raw_block.data[byte_idx + 1] * 256) *
+                        DISTANCE_RESOLUTION * distance_unit;
+
+                //intensity
+                firings.intensity[blk_idx * 32 + scan_idx] = static_cast<double>(
+                        raw_block.data[byte_idx + 2]);
+            }
+
+        }
+        return;
+    }
+
+/** poll the device
+ *  @returns true unless end of file reached
+ */
+    bool lslidarC16Driver::poll(void) {  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+        lslidar_msgs::msg::LslidarPacket::UniquePtr packet(new lslidar_msgs::msg::LslidarPacket());
+
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) {
+                return false;
+            }
+        }
+        const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+
+        //check if the packet is valid
+        if (!checkPacketValidity(raw_packet)) {
+            return false;
+        }
+
+        // packet timestamp
+        if (use_gps_ts) {
+            if (packet_size == 1212) {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = pkt.data[1200] + 2000 - 1900;
+                cur_time.tm_mon = pkt.data[1201] - 1;
+                cur_time.tm_mday = pkt.data[1202];
+                cur_time.tm_hour = pkt.data[1203];
+                cur_time.tm_min = pkt.data[1204];
+                cur_time.tm_sec = pkt.data[1205];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1206] +
+                                  pkt.data[1207] * pow(2, 8) +
+                                  pkt.data[1208] * pow(2, 16) +
+                                  pkt.data[1209] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+
+                if (packet->data[1210] == 0x39) {
+                    return_mode = 2;
+                }
+            } else {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+                cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+                cur_time.tm_mday = this->packetTimeStamp[7];
+                cur_time.tm_hour = this->packetTimeStamp[6];
+                cur_time.tm_min = this->packetTimeStamp[5];
+                cur_time.tm_sec = this->packetTimeStamp[4];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1200] +
+                                  pkt.data[1201] * pow(2, 8) +
+                                  pkt.data[1202] * pow(2, 16) +
+                                  pkt.data[1203] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                //使用gps授时
+                if ((timeStamp.seconds() - timeStamp_bak.seconds()) < 1e-9 &&
+                    (timeStamp.seconds() - timeStamp_bak.seconds()) > -1.0
+                    && is_update_gps_time && packet_time_ns < 100000000) {
+                    timeStamp = rclcpp::Time(packet_time_s + 1, packet_time_ns);
+                } else if ((timeStamp - timeStamp_bak).seconds() > 1.0 && (timeStamp - timeStamp_bak).seconds() < 1.2 &&
+                           is_update_gps_time
+                           && packet_time_ns > 900000000) {
+                    timeStamp = rclcpp::Time(packet_time_s - 1, packet_time_ns);
+                }
+                timeStamp_bak = timeStamp;
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+
+                if (packet->data[1204] == 0x39) {
+                    return_mode = 2;
+                }
+            }
+
+        } else {
+            packet->stamp = get_clock()->now();
+            current_packet_time = rclcpp::Time(packet->stamp).seconds();
+        }
+
+        RCLCPP_INFO_ONCE(this->get_logger(), "return mode: %d", return_mode);
+
+
+        //decode the packet
+        decodePacket(raw_packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (last_azimuth - firings.azimuth[new_sweep_start] > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx]))continue;
+            //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;}  //todo
+            //convert the point to xyz coordinate
+            size_t table_idx = firings.azimuth[fir_idx];
+            double cos_azimuth = cos_azimuth_table[table_idx];
+            double sin_azimuth = sin_azimuth_table[table_idx];
+            double x_coord, y_coord, z_coord;
+            if (coordinate_opt) {
+                int tmp_idx = 1468 - firings.azimuth[fir_idx] < 0 ? 1468 - firings.azimuth[fir_idx] + 36000 : 1468 - firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                          R1_ * cos_azimuth_table[tmp_idx];
+                y_coord = -firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                          R1_ * sin_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+
+            } else {
+                //Y-axis correspondence 0 degree
+                int tmp_idx = firings.azimuth[fir_idx] - 1468 < 0 ? firings.azimuth[fir_idx] -1468 + 36000 : firings.azimuth[fir_idx] -1468;
+                x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                          R1_ * sin_azimuth_table[tmp_idx];
+                y_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                          R1_ * cos_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+            }
+            // computer the time of the point
+            double time = current_packet_time  -
+                          (SCANS_PER_PACKET - fir_idx - 1) * (current_packet_time - last_packet_time) /
+                          (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+
+
+            int remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+
+            sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+            lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            sweep_end_time = current_packet_time - (SCANS_PER_PACKET - end_fir_idx - 1) *
+                                                   (current_packet_time - last_packet_time) / (SCANS_PER_PACKET*1.0);
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            publishPointcloud();
+            if (publish_scan) publishScan();
+
+            sweep_data = lslidar_msgs::msg::LslidarScan::UniquePtr(new lslidar_msgs::msg::LslidarScan());
+            //prepare the next frame scan
+            //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = firings.azimuth[SCANS_PER_PACKET - 1];
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx]))continue;
+                //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;} todo
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                double cos_azimuth = cos_azimuth_table[table_idx];
+                double sin_azimuth = sin_azimuth_table[table_idx];
+                double x_coord, y_coord, z_coord;
+                if (coordinate_opt) {
+                    int tmp_idx = 1468 - firings.azimuth[fir_idx] < 0 ? 1468 - firings.azimuth[fir_idx] + 36000 : 1468 - firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                              R1_ * cos_azimuth_table[tmp_idx];
+                    y_coord = -firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                              R1_ * sin_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - 1468 < 0 ? firings.azimuth[fir_idx] -1468 + 36000 : firings.azimuth[fir_idx] -1468;
+                    x_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * sin_azimuth +
+                              R1_ * sin_azimuth_table[tmp_idx];
+                    y_coord = firings.distance[fir_idx] * c16_cos_scan_altitude[fir_idx % 16] * cos_azimuth +
+                              R1_ * cos_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c16_sin_scan_altitude[fir_idx % 16];
+                }
+
+                // computer the time of the point
+                double time = current_packet_time  -
+                       (SCANS_PER_PACKET - fir_idx -1) * (current_packet_time - last_packet_time) /
+                              (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+
+                int remapped_scan_idx = (fir_idx % 16) % 2 == 0 ? (fir_idx % 16) / 2 : (fir_idx % 16) / 2 + 8;
+                sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+                lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                        sweep_data->points.size() - 1];
+                //pack the data into point msg
+                new_point.time = time;
+                new_point.x = x_coord;
+                new_point.y = y_coord;
+                new_point.z = z_coord;
+                new_point.intensity = firings.intensity[fir_idx];
+                new_point.ring = remapped_scan_idx;
+                new_point.azimuth = firings.azimuth[fir_idx];
+                new_point.distance = firings.distance[fir_idx];
+            }
+
+        }
+        last_packet_time = current_packet_time;
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+
+    lslidarC32Driver::lslidarC32Driver() : lslidarDriver() {
+//        this->initialize();
+        get_difop = false;
+        return;
+    }
+
+    lslidarC32Driver::~lslidarC32Driver() {
+        if (difop_thread_ != nullptr) {
+//            difop_thread_->interrupt();
+            difop_thread_->join();
+        }
+    }
+
+    bool lslidarC32Driver::initialize() {
+        RCLCPP_INFO(this->get_logger(), "lslidarC32Driver::initialize()");
+        this->initTimeStamp();
+        if (!loadParameters()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot load all required ROS parameters.");
+            return false;
+        }
+        if (c32_type == "c32_2") {
+            if (c32_fpga_type == 2) {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_2_vertical_angle_26[j];
+                    c32_config_vertical_angle[j] = c32_2_vertical_angle_26[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            } else {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_2_vertical_angle[j];
+                    c32_config_vertical_angle[j] = c32_2_vertical_angle[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            }
+        } else {
+            if (c32_fpga_type == 2) {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_1_vertical_angle_26[j];
+                    c32_config_vertical_angle[j] = c32_1_vertical_angle_26[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            } else {
+                for (int j = 0; j < 32; ++j) {
+                    c32_vertical_angle[j] = c32_1_vertical_angle[j];
+                    c32_config_vertical_angle[j] = c32_1_vertical_angle[j];
+                    c32_sin_scan_altitude[j] = sin(c32_vertical_angle[j] * DEG_TO_RAD);
+                    c32_cos_scan_altitude[j] = cos(c32_vertical_angle[j] * DEG_TO_RAD);
+                }
+            }
+        }
+
+        if (!createRosIO()) {
+            RCLCPP_ERROR(this->get_logger(), "cannot create all ROS IO.");
+            return false;
+        }
+
+        for (int i = 0; i < 4; ++i) {
+            adjust_angle[i] = 0.0;
+        }
+
+        for (int j = 0; j < 36000; ++j) {
+            sin_azimuth_table[j] = sin(j * 0.01 * DEG_TO_RAD);
+            cos_azimuth_table[j] = cos(j * 0.01 * DEG_TO_RAD);
+        }
+
+        return true;
+    }
+
+    void lslidarC32Driver::difopPoll() {
+        // reading and publishing scans as fast as possible.
+        lslidar_msgs::msg::LslidarPacket::UniquePtr difop_packet_ptr(new lslidar_msgs::msg::LslidarPacket());
+        while (rclcpp::ok()) {
+            //RCLCPP_INFO(this->get_logger(), "423542354354356563");
+            // keep reading
+            int rc = difop_input_->getPacket(difop_packet_ptr);
+            if (rc == 0) {
+                if (difop_packet_ptr->data[0] != 0xa5 || difop_packet_ptr->data[1] != 0xff ||
+                    difop_packet_ptr->data[2] != 0x00 || difop_packet_ptr->data[3] != 0x5a) {
+                    return;
+                }
+                count++;
+                if (count == 2) { is_update_gps_time = true; }
+                for (int i = 0; i < packet_size; i++) {
+                    difop_data[i] = difop_packet_ptr->data[i];
+                }
+                RCLCPP_INFO_ONCE(this->get_logger(), "c32 vertical angle resolution type: %s; c32 fpga type: %0.1f",
+                                 c32_type.c_str(),
+                                 difop_data[1202] + int(difop_data[1203] / 16) * 0.1);
+                //int version_data = difop_packet_ptr->data[1202];
+                if (config_vert) {
+                    if (3 == c32_fpga_type) {
+                        for (int i = 0; i < 32; ++i) {
+                            uint8_t data1 = difop_packet_ptr->data[245 + 2 * i];
+                            uint8_t data2 = difop_packet_ptr->data[245 + 2 * i + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c32_config_tmp_angle[i] = (double) vert_angle * 0.01f;
+
+                        }
+                        for (int j = 0; j < 32; ++j) {
+                            c32_config_vertical_angle[j] = c32_config_tmp_angle[adjust_angle_index[j]];
+
+                            if (fabs(c32_vertical_angle[j] - c32_config_vertical_angle[j]) > 3) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 32; ++k) {
+                                c32_sin_scan_altitude[k] = sin(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                                c32_cos_scan_altitude[k] = cos(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                        // horizontal correction angle
+                        int angle_a0 = difop_packet_ptr->data[186] * 256 + difop_packet_ptr->data[187];
+                        adjust_angle[0] = angle_a0 > 32767 ? 32767 - angle_a0 : angle_a0;
+
+                        int angle_a1 = difop_packet_ptr->data[190] * 256 + difop_packet_ptr->data[191];
+                        adjust_angle[1] = angle_a1 > 32767 ? 32767 - angle_a1 : angle_a1;
+
+                        int angle_a2 = difop_packet_ptr->data[188] * 256 + difop_packet_ptr->data[189];
+                        adjust_angle[2] = angle_a2 > 32767 ? 32767 - angle_a2 : angle_a2;
+
+                        int angle_a3 = difop_packet_ptr->data[192] * 256 + difop_packet_ptr->data[193];
+                        adjust_angle[3] = angle_a3 > 32767 ? 32767 - angle_a3 : angle_a3;
+                    } else {
+                        for (int j = 0; j < 32; ++j) {
+                            uint8_t data1 = difop_packet_ptr->data[882 + 2 * j];
+                            uint8_t data2 = difop_packet_ptr->data[882 + 2 * j + 1];
+                            int vert_angle = data1 * 256 + data2;
+                            vert_angle = vert_angle > 32767 ? (vert_angle - 65536) : vert_angle;
+                            c32_config_vertical_angle[j] = (double) vert_angle * 0.01f;
+
+                            if (fabs(c32_vertical_angle[j] - c32_config_vertical_angle[j]) > 3.0) {
+                                config_vert_num++;
+                            }
+                        }
+                        if (config_vert_num == 0) {
+                            for (int k = 0; k < 32; ++k) {
+                                c32_sin_scan_altitude[k] = sin(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                                c32_cos_scan_altitude[k] = cos(c32_config_vertical_angle[k] * DEG_TO_RAD);
+                            }
+                        }
+                        // horizontal correction angle
+                        int angle_a0 = difop_packet_ptr->data[34] * 256 + difop_packet_ptr->data[35];
+                        adjust_angle[0] = angle_a0 > 32767 ? 32767 - angle_a0 : angle_a0;
+
+                        int angle_a1 = difop_packet_ptr->data[42] * 256 + difop_packet_ptr->data[43];
+                        adjust_angle[1] = angle_a1 > 32767 ? 32767 - angle_a1 : angle_a1;
+
+                        int angle_a2 = difop_packet_ptr->data[66] * 256 + difop_packet_ptr->data[67];
+                        adjust_angle[2] = angle_a2 > 32767 ? 32767 - angle_a2 : angle_a2;
+
+                        int angle_a3 = difop_packet_ptr->data[68] * 256 + difop_packet_ptr->data[69];
+                        adjust_angle[3] = angle_a3 > 32767 ? 32767 - angle_a3 : angle_a3;
+                    }
+                    get_difop = true;
+                    config_vert = false;
+                }
+                if (packet_size == 1206) {
+                    if (2 == c32_fpga_type) {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[41];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[40];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[39];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[38];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[37];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[36];
+                    } else {
+                        this->packetTimeStamp[4] = difop_packet_ptr->data[57];
+                        this->packetTimeStamp[5] = difop_packet_ptr->data[56];
+                        this->packetTimeStamp[6] = difop_packet_ptr->data[55];
+                        this->packetTimeStamp[7] = difop_packet_ptr->data[54];
+                        this->packetTimeStamp[8] = difop_packet_ptr->data[53];
+                        this->packetTimeStamp[9] = difop_packet_ptr->data[52];
+                    }
+                }
+
+            } else if (rc < 0) {
+                return;
+            }
+//            rclcpp::spinOnce();
+        }
+    }
+
+
+    void lslidarC32Driver::decodePacket(const RawPacket *packet) {
+        //couputer azimuth angle for each firing, 12
+
+        for (size_t fir_idx = 0; fir_idx < BLOCKS_PER_PACKET; ++fir_idx) {
+            firings.firing_azimuth[fir_idx] = packet->blocks[fir_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD;
+            // ROS_INFO("azi==%d",packet->blocks[fir_idx].rotation);
+        }
+        // ROS_WARN("-----------------");
+        // computer distance ,intensity
+        //12 blocks
+        for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            const RawBlock &raw_block = packet->blocks[blk_idx];
+
+            int32_t azimuth_diff = 0;
+            if (1 == return_mode) {
+                if (blk_idx < BLOCKS_PER_PACKET - 1) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 1] - firings.firing_azimuth[blk_idx];
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 1];
+                }
+                azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+            } else {
+                if (blk_idx < BLOCKS_PER_PACKET - 2) {
+                    azimuth_diff = firings.firing_azimuth[blk_idx + 2] - firings.firing_azimuth[blk_idx];
+
+                } else {
+                    azimuth_diff = firings.firing_azimuth[blk_idx] - firings.firing_azimuth[blk_idx - 2];
+                }
+                azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff;
+            }
+            // 32 scan
+            for (size_t scan_idx = 0; scan_idx < SCANS_PER_BLOCK; ++scan_idx) {
+                size_t byte_idx = RAW_SCAN_SIZE * scan_idx;
+                //azimuth
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.firing_azimuth[blk_idx] +
+                                                           scan_idx * azimuth_diff / FIRING_TOFFSET;
+
+                firings.azimuth[blk_idx * 32 + scan_idx] = firings.azimuth[blk_idx * 32 + scan_idx] % 36000;
+
+                /*
+                // calibration azimuth ，1°
+              if ("c32_2" == c32_type) {
+                    // -----结果是否是正数 ？
+                    int adjust_diff = adjust_angle[1] - adjust_angle[0];
+                    if (adjust_diff > 300 && adjust_diff < 460) {
+                        // fpga :v 2.7
+                        if (3 == c32_fpga_type) {
+                            if ( 1 >= scan_fir_idx % 4 ) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                           // ROS_INFO("id: %d--azi: %d",blk_idx * 32 + scan_fir_idx,firings.azimuth[blk_idx * 32 + scan_fir_idx]);
+                        } else {
+                            if (0 == scan_fir_idx % 2) {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                            } else {
+                                firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                            }
+                        }
+                    } else {
+                        // fpga: v2.6
+                        if (0 == scan_fir_idx % 2) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        } else {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                    }
+                }
+                //  calibration azimuth ,0.33°
+                if ("c32_1" == c32_type) {
+                    int adjust_diff_one = adjust_angle[1] - adjust_angle[0];
+                    int adjust_diff_two = adjust_angle[3] - adjust_angle[2];
+                    if (3 == c32_fpga_type) {
+                        // fpga v3.0
+                        if (0 == scan_fir_idx || 1 == scan_fir_idx || 4 == scan_fir_idx || 8 == scan_fir_idx ||
+                            9 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 17 == scan_fir_idx || 21 == scan_fir_idx || 24 == scan_fir_idx ||
+                            25 == scan_fir_idx || 29 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (2 == scan_fir_idx || 3 == scan_fir_idx || 6 == scan_fir_idx || 10 == scan_fir_idx ||
+                            11 == scan_fir_idx || 14 == scan_fir_idx
+                            || 18 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx || 26 == scan_fir_idx ||
+                            27 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (5 == scan_fir_idx || 13 == scan_fir_idx || 20 == scan_fir_idx || 28 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (7 == scan_fir_idx || 15 == scan_fir_idx || 22 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else if (adjust_diff_one > 500 && adjust_diff_one < 660 && adjust_diff_two > 150 &&
+                               adjust_diff_two < 350) {
+                        //fpga v2.7
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else {
+                        // fpga v2.6
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[1];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+
+                    }
+
+                }
+
+
+                firings.azimuth[blk_idx * 32 + scan_fir_idx] = firings.azimuth[blk_idx * 32 + scan_fir_idx] % 36000;
+                 */
+
+                // distance
+
+                firings.distance[blk_idx * 32 + scan_idx] =
+                        static_cast<double>(raw_block.data[byte_idx] + raw_block.data[byte_idx + 1] * 256) *
+                        DISTANCE_RESOLUTION * distance_unit;
+
+                //intensity
+                firings.intensity[blk_idx * 32 + scan_idx] = static_cast<double>(
+                        raw_block.data[byte_idx + 2]);
+            }
+
+        }
+        return;
+    }
+
+/** poll the device
+ *  @returns true unless end of file reached
+ */
+    bool lslidarC32Driver::poll(void) {
+        lslidar_msgs::msg::LslidarPacket::UniquePtr packet(new lslidar_msgs::msg::LslidarPacket());
+
+        // Since the rslidar delivers data at a very high rate, keep
+        // reading and publishing scans as fast as possible.
+        while (true) {
+            int rc = msop_input_->getPacket(packet);
+            if (rc == 0) break;
+            if (rc < 0) return false;
+        }
+        const RawPacket *raw_packet = (const RawPacket *) (&(packet->data[0]));
+        //check if the packet is valid
+        if (!checkPacketValidity(raw_packet)) return false;
+        // packet timestamp
+        if (use_gps_ts) {
+            if (packet_size == 1212) {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = pkt.data[1200] + 2000 - 1900;
+                cur_time.tm_mon = pkt.data[1201] - 1;
+                cur_time.tm_mday = pkt.data[1202];
+                cur_time.tm_hour = pkt.data[1203];
+                cur_time.tm_min = pkt.data[1204];
+                cur_time.tm_sec = pkt.data[1205];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1206] +
+                                  pkt.data[1207] * pow(2, 8) +
+                                  pkt.data[1208] * pow(2, 16) +
+                                  pkt.data[1209] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+
+                if (packet->data[1210] == 0x39) {
+                    return_mode = 2;
+                }
+            } else {
+                lslidar_msgs::msg::LslidarPacket pkt = *packet;
+                memset(&cur_time, 0, sizeof(cur_time));
+                cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
+                cur_time.tm_mon = this->packetTimeStamp[8] - 1;
+                cur_time.tm_mday = this->packetTimeStamp[7];
+                cur_time.tm_hour = this->packetTimeStamp[6];
+                cur_time.tm_min = this->packetTimeStamp[5];
+                cur_time.tm_sec = this->packetTimeStamp[4];
+                cur_time.tm_year = cur_time.tm_year >= 200 ? 100 : cur_time.tm_year;
+                packet_time_s = static_cast<uint64_t>(timegm(&cur_time)); //s
+                packet_time_ns = (pkt.data[1200] +
+                                  pkt.data[1201] * pow(2, 8) +
+                                  pkt.data[1202] * pow(2, 16) +
+                                  pkt.data[1203] * pow(2, 24)) * 1e3; //ns
+                timeStamp = rclcpp::Time(packet_time_s, packet_time_ns);
+                RCLCPP_INFO_ONCE(this->get_logger(), "timeStamp: %f", timeStamp.seconds());
+                RCLCPP_INFO_ONCE(this->get_logger(), "timeStamp_bak: %f", timeStamp_bak.seconds());
+                //使用gps授时
+                if ((timeStamp.seconds() - timeStamp_bak.seconds()) < 1e-9 &&
+                    (timeStamp.seconds() - timeStamp_bak.seconds()) > -1.0
+                    && is_update_gps_time && packet_time_ns < 100000000) {
+                    timeStamp = rclcpp::Time(packet_time_s + 1, packet_time_ns);
+                } else if ((timeStamp - timeStamp_bak).seconds() > 1.0 && (timeStamp - timeStamp_bak).seconds() < 1.2 &&
+                           is_update_gps_time
+                           && packet_time_ns > 900000000) {
+                    timeStamp = rclcpp::Time(packet_time_s - 1, packet_time_ns);
+                }
+                timeStamp_bak = timeStamp;
+                packet->stamp = timeStamp;
+                current_packet_time = timeStamp.seconds();
+                if (packet->data[1204] == 0x39) {
+                    return_mode = 2;
+                }
+            }
+        } else {
+            packet->stamp = get_clock()->now();;
+            current_packet_time = rclcpp::Time(packet->stamp).seconds();
+        }
+
+        RCLCPP_INFO_ONCE(this->get_logger(), "return mode: %d", return_mode);
+
+        //decode the packet
+        decodePacket(raw_packet);
+        // find the start of a new revolution
+        // if there is one, new_sweep_start will be the index of the start firing,
+        // otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
+        size_t new_sweep_start = 0;
+        do {
+            if (last_azimuth - firings.azimuth[new_sweep_start] > 35000) {
+                break;
+            } else {
+                last_azimuth = firings.azimuth[new_sweep_start];
+                ++new_sweep_start;
+            }
+        } while (new_sweep_start < SCANS_PER_PACKET);
+
+        // The first sweep may not be complete. So, the firings with
+        // the first sweep will be discarded. We will wait for the
+        // second sweep in order to find the 0 azimuth angle.
+        size_t start_fir_idx = 0;
+        size_t end_fir_idx = new_sweep_start;
+        if (is_first_sweep && new_sweep_start == SCANS_PER_PACKET) {
+            return true;
+        } else {
+            if (is_first_sweep) {
+                is_first_sweep = false;
+                start_fir_idx = new_sweep_start;
+                end_fir_idx = SCANS_PER_PACKET;
+                //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            }
+        }
+        last_azimuth_tmp = firings.azimuth[SCANS_PER_PACKET - 1];
+
+        for (int blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) {
+            for (int scan_fir_idx = 0; scan_fir_idx < SCANS_PER_BLOCK; ++scan_fir_idx) {
+
+                // calibration azimuth ，1°
+                if ("c32_2" == c32_type) {
+
+//                    int adjust_diff = adjust_angle[1] - adjust_angle[0];
+//                    if (adjust_diff > 300 && adjust_diff < 460) {
+                    // fpga :v 2.8 3.0
+                    if (3 == c32_fpga_type) {
+                        if (1 >= scan_fir_idx % 4) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        } else {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        // ROS_INFO("id: %d--azi: %d",blk_idx * 32 + scan_fir_idx,firings.azimuth[blk_idx * 32 + scan_fir_idx]);
+                    }
+//                    }
+                    else {
+                        // fpga: v2.6
+                        if (0 == scan_fir_idx % 2) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        } else {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                    }
+                }
+                //  calibration azimuth ,0.33°
+                if ("c32_1" == c32_type) {
+                  //  int adjust_diff_one = adjust_angle[1] - adjust_angle[0];
+                  //  int adjust_diff_two = adjust_angle[3] - adjust_angle[2];
+                    if (3 == c32_fpga_type) {
+                        // fpga v 2.8 3.0
+                        if (0 == scan_fir_idx || 1 == scan_fir_idx || 4 == scan_fir_idx || 8 == scan_fir_idx ||
+                            9 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 17 == scan_fir_idx || 21 == scan_fir_idx || 24 == scan_fir_idx ||
+                            25 == scan_fir_idx || 29 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[3];
+                        }
+                        if (2 == scan_fir_idx || 3 == scan_fir_idx || 6 == scan_fir_idx || 10 == scan_fir_idx ||
+                            11 == scan_fir_idx || 14 == scan_fir_idx
+                            || 18 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx || 26 == scan_fir_idx ||
+                            27 == scan_fir_idx || 31 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[2];
+                        }
+                        if (5 == scan_fir_idx || 13 == scan_fir_idx || 20 == scan_fir_idx || 28 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (7 == scan_fir_idx || 15 == scan_fir_idx || 22 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                    } else {
+                        // fpga v2.6
+                        if (10 == scan_fir_idx || 14 == scan_fir_idx || 18 == scan_fir_idx || 22 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[0];
+                        }
+                        if (11 == scan_fir_idx || 15 == scan_fir_idx || 19 == scan_fir_idx || 23 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[0];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+                        if (0 == scan_fir_idx || 2 == scan_fir_idx || 4 == scan_fir_idx || 6 == scan_fir_idx ||
+                            8 == scan_fir_idx || 12 == scan_fir_idx
+                            || 16 == scan_fir_idx || 20 == scan_fir_idx || 24 == scan_fir_idx || 26 == scan_fir_idx ||
+                            28 == scan_fir_idx || 30 == scan_fir_idx) {
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] += adjust_angle[1];
+                        }
+                        if (1 == scan_fir_idx || 3 == scan_fir_idx || 5 == scan_fir_idx || 7 == scan_fir_idx ||
+                            9 == scan_fir_idx || 13 == scan_fir_idx
+                            || 17 == scan_fir_idx || 21 == scan_fir_idx || 25 == scan_fir_idx || 27 == scan_fir_idx ||
+                            29 == scan_fir_idx || 31 == scan_fir_idx) {
+                            int tmp_azimuth = firings.azimuth[blk_idx * 32 + scan_fir_idx] - adjust_angle[1];
+                            firings.azimuth[blk_idx * 32 + scan_fir_idx] =
+                                    tmp_azimuth < 0 ? tmp_azimuth + 36000 : tmp_azimuth;
+                        }
+
+                    }
+
+                }
+                if (firings.azimuth[blk_idx * 32 + scan_fir_idx] < 0)
+                    firings.azimuth[blk_idx * 32 + scan_fir_idx] += 36000;
+                if (firings.azimuth[blk_idx * 32 + scan_fir_idx] > 36000)
+                    firings.azimuth[blk_idx * 32 + scan_fir_idx] -= 36000;
+            }
+
+        }
+
+
+        for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+            //check if the point is valid
+            if (!isPointInRange(firings.distance[fir_idx]))continue;
+            //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;}  // todo
+            //convert the point to xyz coordinate
+            firings.azimuth[fir_idx] =
+                    firings.azimuth[fir_idx] >= 36000 ? firings.azimuth[fir_idx] - 36000 : firings.azimuth[fir_idx];
+            size_t table_idx = firings.azimuth[fir_idx];
+            double cos_azimuth = cos_azimuth_table[table_idx];
+            double sin_azimuth = sin_azimuth_table[table_idx];
+            double x_coord, y_coord, z_coord;
+            if (coordinate_opt) {
+                //X-axis correspondence 0 degree
+                int tmp_idx = 1298 - firings.azimuth[fir_idx] < 0 ? 1298 - firings.azimuth[fir_idx] + 36000 : 1298 - firings.azimuth[fir_idx];
+                x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                          R2_ * cos_azimuth_table[tmp_idx];
+                y_coord = -firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                          R2_ * sin_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+            } else {
+                //Y-axis correspondence 0 degree
+                int tmp_idx = firings.azimuth[fir_idx] - 1298 < 0 ? firings.azimuth[fir_idx] -1298 + 36000 : firings.azimuth[fir_idx] -1298;
+                x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                          R2_ * sin_azimuth_table[tmp_idx];
+                y_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                          R2_ * cos_azimuth_table[tmp_idx];
+                z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+            }
+            // computer the time of the point
+            double time = current_packet_time  -
+                          (SCANS_PER_PACKET - fir_idx - 1) * (current_packet_time - last_packet_time) /
+                          (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+            int remapped_scan_idx;
+            if (c32_fpga_type == 2) {
+                remapped_scan_idx = fir_idx % 32;
+            } else {
+                remapped_scan_idx = (fir_idx % 32) % 2 == 0 ? (fir_idx % 32) / 2 : (fir_idx % 32) / 2 + 16;
+            }
+            sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+            lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                    sweep_data->points.size() - 1];
+            //pack the data into point msg
+            new_point.time = time;
+            new_point.x = x_coord;
+            new_point.y = y_coord;
+            new_point.z = z_coord;
+            new_point.intensity = firings.intensity[fir_idx];
+            new_point.ring = remapped_scan_idx;
+            new_point.azimuth = firings.azimuth[fir_idx];
+            new_point.distance = firings.distance[fir_idx];
+        }
+        // a new sweep begins ----------------------------------------------------
+
+        if (end_fir_idx != SCANS_PER_PACKET) {
+            //publish Last frame scan
+
+//            sweep_end_time = rclcpp::Time(packet->stamp).seconds() -
+//                             static_cast<double>(SCANS_PER_PACKET - end_fir_idx) * 3.125 * 1e-6;
+            sweep_end_time = current_packet_time - (SCANS_PER_PACKET - end_fir_idx - 1) *
+                                                   (current_packet_time - last_packet_time) / (SCANS_PER_PACKET *1.0);
+            sweep_end_time = sweep_end_time > 0 ? sweep_end_time : 0;
+            if (get_difop) publishPointcloud();
+            if (publish_scan) publishScan();
+
+            sweep_data = lslidar_msgs::msg::LslidarScan::UniquePtr(new lslidar_msgs::msg::LslidarScan());
+            //prepare the next frame scan
+            //sweep_start_time = packet->stamp.toSec() - (end_fir_idx - start_fir_idx) * 3.125 * 1e-6;
+            last_azimuth = last_azimuth_tmp;
+            start_fir_idx = end_fir_idx;
+            end_fir_idx = SCANS_PER_PACKET;
+            for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
+                //check if the point is valid
+                if (!isPointInRange(firings.distance[fir_idx]))continue;
+                //if (firings.azimuth[fir_idx]>=36000) { firings.azimuth[fir_idx]-=36000;}  // todo
+                firings.azimuth[fir_idx] =
+                        firings.azimuth[fir_idx] >= 36000 ? firings.azimuth[fir_idx] - 36000 : firings.azimuth[fir_idx];
+                //convert the point to xyz coordinate
+                size_t table_idx = firings.azimuth[fir_idx];
+                double cos_azimuth = cos_azimuth_table[table_idx];
+                double sin_azimuth = sin_azimuth_table[table_idx];
+                double x_coord, y_coord, z_coord;
+                if (coordinate_opt) {
+                    //X-axis correspondence 0 degree
+                    int tmp_idx = 1298 - firings.azimuth[fir_idx] < 0 ? 1298 - firings.azimuth[fir_idx] + 36000 : 1298 - firings.azimuth[fir_idx];
+                    x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                              R2_ * cos_azimuth_table[tmp_idx];
+                    y_coord = -firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                              R2_ * sin_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+
+                } else {
+                    //Y-axis correspondence 0 degree
+                    int tmp_idx = firings.azimuth[fir_idx] - 1298 < 0 ? firings.azimuth[fir_idx] -1298 + 36000 : firings.azimuth[fir_idx] -1298;
+                    x_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * sin_azimuth +
+                              R2_ * sin_azimuth_table[tmp_idx];
+                    y_coord = firings.distance[fir_idx] * c32_cos_scan_altitude[fir_idx % 32] * cos_azimuth +
+                              R2_ * cos_azimuth_table[tmp_idx];
+                    z_coord = firings.distance[fir_idx] * c32_sin_scan_altitude[fir_idx % 32];
+                }
+                // computer the time of the point
+//                double time = rclcpp::Time(packet->stamp).seconds() - (SCANS_PER_PACKET - fir_idx) * 3.125 * 1e-6;
+                double time = current_packet_time  -
+                              (SCANS_PER_PACKET - fir_idx - 1) * (current_packet_time - last_packet_time) /
+                              (SCANS_PER_PACKET * 1.0) - sweep_end_time;
+                int remapped_scan_idx;
+                if (c32_fpga_type == 2) {
+                    remapped_scan_idx = fir_idx % 32;
+                } else {
+                    remapped_scan_idx = (fir_idx % 32) % 2 == 0 ? (fir_idx % 32) / 2 : (fir_idx % 32) / 2 + 16;
+                }
+                sweep_data->points.push_back(lslidar_msgs::msg::LslidarPoint());
+                lslidar_msgs::msg::LslidarPoint &new_point = sweep_data->points[
+                        sweep_data->points.size() - 1];
+                //pack the data into point msg
+                new_point.time = time;
+                new_point.x = x_coord;
+                new_point.y = y_coord;
+                new_point.z = z_coord;
+                new_point.intensity = firings.intensity[fir_idx];
+                new_point.ring = remapped_scan_idx;
+                new_point.azimuth = firings.azimuth[fir_idx];
+                new_point.distance = firings.distance[fir_idx];
+            }
+
+        }
+        last_packet_time = current_packet_time;
+        //packet_pub.publish(*packet);
+        return true;
+    }
+
+
+}  // namespace lslidar_driver

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_driver/src/lslidar_driver_node.cpp

@@ -0,0 +1,47 @@
+#include "lslidar_driver/lslidar_driver.h"
+#include "std_msgs/msg/string.h"
+
+using namespace lslidar_driver;
+volatile sig_atomic_t flag = 1;
+rclcpp::Node::SharedPtr node_p = nullptr;
+
+static void my_handler(int sig) {
+    flag = 0;
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    signal(SIGINT, my_handler);
+    lidar_type = "c32";
+    node_p = rclcpp::Node::make_shared("test");
+    // node_p = std::make_shared<rclcpp::Node>();
+    node_p->declare_parameter("lidar_type","c16");
+    node_p->get_parameter("lidar_type", lidar_type);
+    //printf("lslidar type: %s", lidar_type.c_str());
+    RCLCPP_WARN(node_p->get_logger(),"lslidar type-----: %s", lidar_type.c_str());
+    // start the driver
+    if ("c16" == lidar_type) {
+        auto node = std::make_shared<lslidar_driver::lslidarC16Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+
+        }
+        rclcpp::shutdown();
+
+    } else {
+        auto node = std::make_shared<lslidar_driver::lslidarC32Driver>();
+        if (!node->initialize()) {
+            RCLCPP_ERROR(node->get_logger(), "cannot initialize lslidar driver.");
+            return 0;
+        }
+        while (rclcpp::ok()) {
+            node->poll();
+        }
+        rclcpp::shutdown();
+    }
+    return 0;
+}

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_msgs/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(lslidar_msgs)
+
+set(CMAKE_BUILD_TYPE Release)    #RelWithDebInfo00
+# 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/LslidarPacket.msg"
+        "msg/LslidarPoint.msg"
+        "msg/LslidarScan.msg"
+        "srv/Lslidarcontrol.srv"
+        DEPENDENCIES builtin_interfaces std_msgs sensor_msgs
+        )
+
+ament_package()

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPacket.msg

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

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_msgs/msg/LslidarPoint.msg

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

src/ros2/AutoWare2025/src/Lslidar_ROS2_driver/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/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>lslidar_msgs</name>
+  <version>3.0.1</version>
+  <description>ROS message definitions for C32 LIDARs.</description>
+  <url>http://www.ros.org/wiki/lidar_c32</url>
+  <maintainer email="yutongyan@lidar.com">Yutong</maintainer>
+  <author>Yutong</author>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_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/ros2/AutoWare2025/src/Lslidar_ROS2_driver/lslidar_msgs/srv/Lslidarcontrol.srv

@@ -0,0 +1,3 @@
+int32 lasercontrol
+---
+bool status

src/ros2/AutoWare2025/src/adc_autoware_msgs/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(adc_autoware_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/AdcCanFrame.msg"
+  "msg/AdcCanMsgs.msg"
+  "msg/AdcChassis.msg"
+  "srv/ChassisEnableCtrl.srv"
+  DEPENDENCIES std_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ros2/AutoWare2025/src/adc_autoware_msgs/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/ros2/AutoWare2025/src/adc_autoware_msgs/msg/AdcCanFrame.msg

@@ -0,0 +1,7 @@
+std_msgs/Header header
+uint32 id
+bool is_rtr
+bool is_extended
+bool is_error
+uint8 dlc
+uint8[64] data

src/ros2/AutoWare2025/src/adc_autoware_msgs/msg/AdcCanMsgs.msg

@@ -0,0 +1,4 @@
+
+std_msgs/Header header
+adc_autoware_msgs/AdcCanFrame[] frames
+

src/ros2/AutoWare2025/src/adc_autoware_msgs/msg/AdcChassis.msg

@@ -0,0 +1,27 @@
+
+std_msgs/Header header
+int32 epsmode
+int32 epbfault
+int32 drivemode
+int32 shift
+int32 aebavailable
+int32 cddavailable
+int32 angle_feedback
+float32 torque
+float32 vel
+float32 accstep
+float32 soc
+float32 brake_feedback
+int32 epb_feedback
+int32 emergencystop_feedback
+int32 brakelight_feedback
+float32 range_feedback
+int32 drivectrltype_feedback
+int32 brakectrltype_feedback
+int32 epsctrltype_feedback
+float32 frontleftwheel_feedback
+float32 frontrightwheel_feedback
+float32 rearleftwheel_feedback
+float32 rearrightwheel_feedback
+float32 engine_speed
+

src/ros2/AutoWare2025/src/adc_autoware_msgs/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>adc_autoware_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/AutoWare2025/src/adc_autoware_msgs/srv/ChassisEnableCtrl.srv

@@ -0,0 +1,3 @@
+bool benable
+---
+bool feedback_enable

src/ros2/AutoWare2025/src/adc_autowaremodule_launch/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(adc_autowaremodule_launch)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+  )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ros2/AutoWare2025/src/adc_autowaremodule_launch/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/ros2/AutoWare2025/src/adc_autowaremodule_launch/launch/modulelaunch.py

@@ -0,0 +1,42 @@
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="adc_gps_hcp2",
+            executable="adc_gps_hcp2",
+            name="adc_gps_hcp2",
+            output="screen",
+            emulate_tty=True,
+            parameters=[
+                {"SerialPortName": "/dev/ttyUSB0"},
+                {"UseVelocity": True}
+            ]
+        ),
+
+        Node(
+            package="adc_controller_shenlan_v2",
+            executable="adc_controller_shenlan_v2",
+            name="adc_controller_shenlan_v2",
+            output="screen",
+            emulate_tty=True
+        ),
+
+        Node(
+            package="adc_can_nvidia_agx",
+            executable="adc_can_nvidia_agx",
+            name="adc_can_nvidia_agx",
+            output="screen",
+            emulate_tty=True
+        ),
+        Node(
+            package="adc_chassis",
+            executable="adc_chassis",
+            name="adc_chassis",
+            output="screen",
+            emulate_tty=True
+        )
+    ])
+

src/ros2/AutoWare2025/src/adc_autowaremodule_launch/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>adc_autowaremodule_launch</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.14)
+project(adc_can_nvidia_agx)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+
+#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(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED) 
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+find_package(adc_autoware_msgs REQUIRED)
+
+find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
+
+find_package(Protobuf REQUIRED)
+
+
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include/adc_can_nvidia_agx
+)
+link_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../lib
+)
+
+
+add_executable(adc_can_nvidia_agx
+  src/adc_can_nvidia_agx_node.cpp
+  src/adc_can_nvidia_agx_core.cpp
+  src/basecan.cpp
+  src/canctrl.cpp
+  src/nvcan.cpp
+)
+
+#target_link_libraries(adc_can_nvidia_agx  ${Protobuf_LIBRARIES}  Geographic )  
+# modulecomm 
+
+ament_target_dependencies(adc_can_nvidia_agx rclcpp std_msgs geometry_msgs 
+  tf2_geometry_msgs nav_msgs sensor_msgs can_msgs autoware_control_msgs  Qt5Core
+ adc_autoware_msgs)
+
+
+install(TARGETS
+  adc_can_nvidia_agx
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
+
+

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/README.md

@@ -0,0 +1,31 @@
+# pilot_autoware_bridge
+
+## Purpose
+
+This `pilot_autoware_bridge` is a bridge between pilot and autoware.
+
+
+
+## Inputs / Outputs
+
+### Input
+
+| Name | Type                            | Description                                       |
+| ---- | ------------------------------- | ------------------------------------------------- |
+| pose | geometry_msgs::msg::PoseStamped | pose source to used for the velocity calculation. |
+
+### Output
+
+| Name      | Type                                  | Description                                   |
+| --------- | ------------------------------------- | --------------------------------------------- |
+| twist     | geometry_msgs::msg::TwistStamped      | twist calculated from the input pose history. |
+| linear_x  | tier4_debug_msgs::msg::Float32Stamped | linear-x field of the output twist.           |
+| angular_z | tier4_debug_msgs::msg::Float32Stamped | angular-z field of the output twist.          |
+
+## Parameters
+
+none.
+
+## Assumptions / Known limits
+
+none.

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/include/adc_can_nvidia_agx/adc_can_nvidia_agx_core.hpp

@@ -0,0 +1,67 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ADC_CAN_NVIDIA_AGX_CORE_HPP_
+#define ADC_CAN_NVIDIA_AGX_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "can_msgs/msg/frame.hpp"
+
+#include <string>
+
+#include "nvcan.h"
+#include "canctrl.h"
+
+#include "adc_autoware_msgs/msg/adc_can_frame.hpp"
+#include "adc_autoware_msgs/msg/adc_can_msgs.hpp"
+
+
+class adc_can_nvidia_agx : public rclcpp::Node
+{
+public:
+    adc_can_nvidia_agx();
+    ~adc_can_nvidia_agx();
+
+private:
+
+
+
+private:
+    std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+    std::string origin_frame_id_;     //!< @brief map frame_id
+
+    canctrl * mpcantrl;
+    
+
+private:
+    
+//   rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_test;
+//    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_canrecv0;
+    rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_canrecv1;
+
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_msgs_cansend0;
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_msgs_cansend1;
+
+private:
+    void timer_callback();
+
+    void topic_cansend0_callback(const adc_autoware_msgs::msg::AdcCanMsgs & msg) const;
+    void topic_cansend1_callback(const adc_autoware_msgs::msg::AdcCanMsgs & msg) const;
+
+
+};
+
+#endif  

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/include/adc_can_nvidia_agx/basecan.h

@@ -0,0 +1,31 @@
+#ifndef BASECAN_H
+#define BASECAN_H
+
+#include <thread>
+
+class basecan_msg
+{
+  public:
+    unsigned int id;
+    bool isExtern;
+    bool isRemote;
+    unsigned char nLen;
+    unsigned char data[64];
+    int64_t nRecvTime;
+};
+
+class basecan
+{
+public:
+    basecan();
+    virtual ~basecan();
+    virtual int GetMessage(const int nch,basecan_msg * pMsg,const int nCap);
+    virtual int SetMessage(const int nch,basecan_msg * pMsg); //Send Message
+
+    virtual void startdev();
+    virtual void stopdev();
+
+    virtual bool IsOpen();
+};
+
+#endif // BASECAN_H

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/include/adc_can_nvidia_agx/canctrl.h

@@ -0,0 +1,74 @@
+#ifndef CANCTRL_H
+#define CANCTRL_H
+
+//#include <QThread>
+#include <QMutex>
+#include <memory>
+#include <QTimer>
+#include <array>
+#include <vector>
+#include <iostream>
+#include <thread>
+#include "nvcan.h"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "adc_autoware_msgs/msg/adc_can_frame.hpp"
+#include "adc_autoware_msgs/msg/adc_can_msgs.hpp"
+
+
+class canctrl
+{
+public:
+    canctrl(rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_send0,
+            rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_send1,
+            rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_recv0,
+            rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_recv1);
+    ~canctrl();
+
+private slots:
+    void onCANState(bool bCAN,int nR,const char * strres);
+
+private:
+    void run();
+
+    basecan * mpcan;
+    std::shared_ptr<basecan> mspcan;
+    bool mbCANOpen = false;
+
+    std::vector<basecan_msg> msendmsgvector1;
+    std::vector<basecan_msg> msendmsgvector2;
+    const int SENDMSGBUFSIZE = 3000;
+
+    QMutex mMutexcan1;
+    QMutex mMutexcan2;
+
+    int mindex[2];
+
+    void * mpasend0, * mpasend1, * mparecv0, * mparecv1, *mpcanState;
+
+    std::thread * mpthread;
+    bool mbthreadrun = true;
+
+public:
+
+    void sharecanmsg(rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pubcanrecv,basecan_msg * pxmsg,int ncount,int nch);
+
+    void sendmsg(int index,const adc_autoware_msgs::msg::AdcCanMsgs & xmsg);
+
+private:
+    
+    rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_canrecv0;
+    rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_canrecv1;
+
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_msgs_cansend0;
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_msgs_cansend1;
+
+private:
+    void testpubthread();
+    std::thread * mpthreadtest;
+
+
+};
+
+#endif // CANCTRL_H

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/include/adc_can_nvidia_agx/nvcan.h

@@ -0,0 +1,41 @@
+#ifndef NVCAN_H
+#define NVCAN_H
+#include "basecan.h"
+
+#include <vector>
+#include <QMutex>
+
+#include <thread>
+
+class nvcan : public basecan
+{
+public:
+    nvcan();
+public:
+    void startdev();
+    void stopdev();
+
+    int GetMessage(const int nch,basecan_msg * pMsg,const int nCap);
+    int SetMessage(const int nch,basecan_msg * pMsg);
+
+    bool IsOpen();
+
+private:
+    void run();
+    std::vector<basecan_msg> mMsgRecvBuf[2];
+
+    std::vector<basecan_msg> mMsgSendBuf[2];
+
+    QMutex mMutex;
+
+    bool mbCANOpen = false;
+    bool mbRunning = false;
+    int mnDevNum;
+
+    std::thread * mpthread;
+    bool mbthreadrun = true;
+
+
+};
+
+#endif // NVCAN_H

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/launch/adc_can_nvidia_agx.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input_pose_topic" default="/localization/pose_estimator/pose" description=""/>
+  <arg name="output_twist_topic" default="/estimate_twist" description=""/>
+
+  <node pkg="testpose" exec="testpose" name="testpose" output="log">
+    <remap from="pose" to="$(var input_pose_topic)"/>
+    <remap from="twist" to="$(var output_twist_topic)"/>
+  </node>
+</launch>

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/package.xml

@@ -0,0 +1,36 @@
+<?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>adc_can_nvidia_agx</name>
+  <version>0.1.0</version>
+  <description>NVIDIA AGX CAN Driver for ros2</description>
+  <maintainer email="yuchuli@catarc.ac.cn">Yu Chuli</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="yuchuli@catarc.ac.cn">Yu Chuli</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+
+    <depend>tier4_api_utils</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
+  <depend>tier4_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>can_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>adc_autoware_msgs</depend>
+
+
+
+ <export>
+   <build_type>ament_cmake</build_type>
+ </export>
+</package>

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/src/adc_can_nvidia_agx_core.cpp

@@ -0,0 +1,111 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "adc_can_nvidia_agx/adc_can_nvidia_agx_core.hpp"
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <cmath>
+#include <cstddef>
+#include <functional>
+
+#include <time.h>
+#include <sstream>
+
+#include <iostream>
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+#include <QString>
+
+#include <functional>
+#include <memory>                      
+
+using std::placeholders::_1;
+
+
+//#include "modulecomm.h"   
+using namespace std;
+
+
+adc_can_nvidia_agx::adc_can_nvidia_agx() : Node("adc_can_nvidia_agx")
+{
+
+
+    simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
+    origin_frame_id_ = declare_parameter("origin_frame_id", "map");
+
+    static constexpr std::size_t queue_size = 1;
+    rclcpp::QoS durable_qos(queue_size);
+    durable_qos.transient_local();
+
+
+    pub_msgs_canrecv0 = this->create_publisher<adc_autoware_msgs::msg::AdcCanMsgs>("canrecv0",10);
+    pub_msgs_canrecv1 = this->create_publisher<adc_autoware_msgs::msg::AdcCanMsgs>("canrecv1",10);
+
+    sub_msgs_cansend0 = this->create_subscription<adc_autoware_msgs::msg::AdcCanMsgs>("cansend0",10,
+                            std::bind(&adc_can_nvidia_agx::topic_cansend0_callback,this,_1));
+    sub_msgs_cansend1 = this->create_subscription<adc_autoware_msgs::msg::AdcCanMsgs>("cansend1",10,
+                            std::bind(&adc_can_nvidia_agx::topic_cansend1_callback,this,_1));
+
+    mpcantrl = new canctrl(sub_msgs_cansend0,sub_msgs_cansend1,pub_msgs_canrecv0,pub_msgs_canrecv1);
+
+
+
+ //   pub_msgs_test = this->create_publisher<adc_autoware_msgs::msg::AdcCanMsgs>("topic", 10);  // CHANGE
+ //   timer_ = this->create_wall_timer(
+ //     500ms, std::bind(&adc_can_nvidia_agx::timer_callback, this));
+
+}
+
+adc_can_nvidia_agx::~adc_can_nvidia_agx()
+{
+    delete mpcantrl;
+}
+
+void adc_can_nvidia_agx::timer_callback()
+{
+    std::cout<<" on timer:"<<std::endl;
+
+/*
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    int64_t nSecond = nnow/1000000000;
+    int64_t nPartSecond = nnow - nSecond * 1000000000;
+
+    adc_autoware_msgs::msg::AdcCanMsgs xmsg;
+    xmsg.header.frame_id = "adc";
+    xmsg.header.stamp.sec = nSecond;
+    xmsg.header.stamp.nanosec = nPartSecond;
+*/
+
+}
+
+void adc_can_nvidia_agx::topic_cansend0_callback(const adc_autoware_msgs::msg::AdcCanMsgs & xmsg) const
+{
+    (void)xmsg;
+ //   std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" Recv A Can Msg."<<std::endl;
+    mpcantrl->sendmsg(0,xmsg);
+}
+
+void adc_can_nvidia_agx::topic_cansend1_callback(const adc_autoware_msgs::msg::AdcCanMsgs & xmsg) const
+{
+    (void)xmsg;
+    mpcantrl->sendmsg(1,xmsg);
+}
+

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/src/adc_can_nvidia_agx_node.cpp

@@ -0,0 +1,30 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "adc_can_nvidia_agx/adc_can_nvidia_agx_core.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<adc_can_nvidia_agx>());
+
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/src/basecan.cpp

@@ -0,0 +1,46 @@
+#include "adc_can_nvidia_agx/basecan.h"
+
+#include  <iostream>
+
+
+basecan::basecan()
+{
+
+}
+
+basecan::~basecan()
+{
+    std::cout<<" del basecan"<<std::endl;
+
+}
+
+int basecan::GetMessage(const int nch,basecan_msg *pMsg, const int nCap)
+{
+    (void)nch;
+    (void)pMsg;
+    (void)nCap;
+    return 0;
+}
+
+int basecan::SetMessage(const int nch, basecan_msg *pMsg)
+{
+    (void)nch;
+    (void)pMsg;
+    return 0;
+}
+
+
+void basecan::startdev()
+{
+
+}
+
+void basecan::stopdev()
+{
+
+}
+
+bool basecan::IsOpen()
+{
+    return false;
+}

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/src/canctrl.cpp

@@ -0,0 +1,282 @@
+#include "adc_can_nvidia_agx/canctrl.h"
+
+#include <memory>
+
+#include <QTime>
+
+canctrl * gc;
+
+
+
+
+canctrl::canctrl(rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_send0,
+            rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_send1,
+            rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_recv0,
+            rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_recv1)
+{
+    sub_msgs_cansend0 = sub_send0;
+    sub_msgs_cansend1 = sub_send1;
+    pub_msgs_canrecv0 = pub_recv0;
+    pub_msgs_canrecv1 = pub_recv1;
+
+
+    mpthread = new std::thread(&canctrl::run,this);
+
+    mpcan = new nvcan();
+    mspcan.reset(mpcan);
+ //   connect(mpcan,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onCANState(bool,int,const char*)));
+    mpcan->startdev();
+
+
+ //   mpthreadtest = new std::thread(&canctrl::testpubthread,this);
+}
+
+canctrl::~canctrl()
+{
+    mbthreadrun = false;
+    mpthread->join();
+    mpcan->stopdev();
+    delete mpcan;
+}
+
+void canctrl::run()
+{
+
+    int nOldTime =  std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    int i;
+    (void)nOldTime;
+
+    static bool bTip = false;
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
+
+    while(mbthreadrun)
+    {
+        mbCANOpen = mpcan->IsOpen();
+        if(mbCANOpen)
+        {
+            if(bTip == true)
+            {
+                bTip = false;
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" Open CAN SuccessFully."<<std::endl;
+            }
+            basecan_msg xmsg[2500];
+            int nRec1,nRec2,nSend1,nSend2;
+            (void)nSend1;
+            (void)nSend2;
+            if((nRec1 =mpcan->GetMessage(0,xmsg,2500))>0)
+            {
+                sharecanmsg(pub_msgs_canrecv0,xmsg,nRec1,0);
+ //               sharecanmsg(mparecv0,xmsg,nRec1,0);
+            }
+
+            if((nRec2 =mpcan->GetMessage(1,xmsg,2500))>0)
+            {
+                sharecanmsg(pub_msgs_canrecv1,xmsg,nRec2,1);
+  //              sharecanmsg(mparecv1,xmsg,nRec2,1);
+            }
+
+
+            nSend1 = 0;
+            nSend2 = 0;
+
+            mMutexcan1.lock();
+            for(i=0;i<static_cast<int>( msendmsgvector1.size());i++)
+            {
+                mpcan->SetMessage(0,&(msendmsgvector1.at(i)));
+            }
+            msendmsgvector1.clear();
+            mMutexcan1.unlock();
+
+            mMutexcan2.lock();
+            for(i=0; i<static_cast<int>(msendmsgvector2.size());i++)
+            {
+                mpcan->SetMessage(1,&(msendmsgvector2.at(i)));
+            }
+            msendmsgvector2.clear();
+            mMutexcan2.unlock();
+
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        else
+        {
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            
+            if(bTip == false)
+            {
+                bTip = true;
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" open can fail."<<std::endl;
+            }
+            
+
+        }
+//        mpcan->mfault->SetFaultState(0, 0, "ok");
+    }
+
+    qDebug("thread canctrl complete.");
+
+}
+
+void canctrl::onCANState(bool bCAN, int nR, const char *strres)
+{
+    (void)nR;
+    mbCANOpen = bCAN;
+
+    std::cout<<" canstate is : "<<strres<<std::endl;
+}
+
+
+void canctrl::sharecanmsg(rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pubcanrecv,basecan_msg * pxmsg,int ncount,int nch)
+{
+    (void)pxmsg;
+    (void)ncount;
+    (void)nch;
+
+    auto message = adc_autoware_msgs::msg::AdcCanMsgs();
+    message.header.frame_id = "adc";
+    int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
+    message.header.stamp.sec = now/1000000000;
+    message.header.stamp.nanosec = now - message.header.stamp.sec * 1e9;
+    int i;
+    for(i=0;i<ncount;i++)
+    {
+        auto frame1 = adc_autoware_msgs::msg::AdcCanFrame();
+        frame1.header.frame_id = "adc";
+        frame1.header.stamp.sec = pxmsg[i].nRecvTime/1000000000;
+        frame1.header.stamp.nanosec = pxmsg[i].nRecvTime - message.header.stamp.sec * 1e9;
+        frame1.id = pxmsg[i].id;
+        frame1.dlc = pxmsg[i].nLen;
+        frame1.is_rtr = pxmsg[i].isExtern;
+        frame1.is_extended = pxmsg[i].isRemote;
+        frame1.is_error = false;
+        if(pxmsg[i].nLen<=64)
+        {
+            if(pxmsg[i].nLen>0)memcpy(frame1.data.data(),pxmsg[i].data,pxmsg[i].nLen);
+        }
+        else
+        {
+            std::cout<<" convert canmsg to ros2 message fail, because data len more than 64. len:"<<pxmsg[i].nLen<<std::endl;
+        }
+        message.frames.push_back(frame1);
+    }
+
+  //  std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" ch: "<<nch<<"  publish msg . size: "<<ncount<<std::endl;
+    pubcanrecv->publish(message);
+}
+
+void canctrl::testpubthread()
+{
+    while(mbthreadrun)
+    {
+        auto message = adc_autoware_msgs::msg::AdcCanMsgs();
+        message.header.frame_id = "adc";
+        int64_t now = std::chrono::system_clock::now().time_since_epoch().count();
+        message.header.stamp.sec = now/1000000000;
+        message.header.stamp.nanosec = now - message.header.stamp.sec * 1e9;
+
+        auto frame1 = adc_autoware_msgs::msg::AdcCanFrame();
+        frame1.header.frame_id = "adc";
+        now = std::chrono::system_clock::now().time_since_epoch().count();
+        frame1.header.stamp.sec = now/1000000000;
+        frame1.header.stamp.nanosec = now - message.header.stamp.sec * 1e9;
+        frame1.id = 0x123;
+        frame1.dlc = 8;
+        frame1.is_rtr = false;
+        frame1.is_extended = false;
+        frame1.is_error = false;
+        frame1.data[0] = 0x01;
+        unsigned char strdata[8];
+        strdata[0] = 0x11;
+        strdata[1] = 0x22;
+        strdata[2] = 0x33;
+        strdata[3] = 0x44;
+        strdata[4] = 0x55;
+        strdata[5] = 0x66;
+        strdata[6] = 0x77;
+        strdata[7] = 0x88;
+        memcpy(frame1.data.data(),strdata,8);
+
+        auto frame2 = adc_autoware_msgs::msg::AdcCanFrame();
+        frame2.header.frame_id = "adc";
+        now = std::chrono::system_clock::now().time_since_epoch().count();
+        frame2.header.stamp.sec = now/1000000000;
+        frame2.header.stamp.nanosec = now - message.header.stamp.sec * 1e9;
+        frame2.id = 0x126;
+        frame2.dlc = 8;
+        frame2.is_rtr = false;
+        frame2.is_extended = false;
+        frame2.is_error = false;
+        frame2.data[0]= 0x02;
+
+        message.frames.push_back(frame1);
+        message.frames.push_back(frame2);
+
+        pub_msgs_canrecv0->publish(message);
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
+}
+
+void canctrl::sendmsg(int index,const adc_autoware_msgs::msg::AdcCanMsgs & xmsg)
+{
+    if((index <0)||(index > 1))
+    {
+        std::cout<<"canctrl::sendmsg index error. index: "<<index<<std::endl;
+        return;
+    }
+
+    std::vector<basecan_msg> * psendmsgvector;
+    QMutex * pMutex;
+    if(index == 0)
+    {
+        pMutex = &mMutexcan1;
+        psendmsgvector = &msendmsgvector1;
+    }
+    if(index == 1)
+    {
+        pMutex = &mMutexcan2;
+        psendmsgvector = &msendmsgvector2;
+    }
+
+    if(static_cast<int>( psendmsgvector->size()) > SENDMSGBUFSIZE)
+    {
+        std::cout<<"sendmsg buf full."<<std::endl;
+        return;
+    }
+
+
+    pMutex->lock();
+    if(psendmsgvector->size() > 1000)psendmsgvector->clear();
+    int i;
+    int ncount = static_cast<int>(xmsg.frames.size());
+    for(i=0;i<ncount;i++)
+    {
+        basecan_msg sendmsg;
+        auto frame = xmsg.frames[i];
+        sendmsg.id = frame.id;
+        sendmsg.isExtern = frame.is_extended;
+        sendmsg.isRemote = frame.is_rtr;
+        int nlen = frame.dlc;
+
+        if((nlen < 0) || (nlen > 64))
+        {
+            nlen = 0;
+            std::cout<<"sendmsg nlen err; len:"<<nlen<<std::endl;
+            continue;
+ //           mpcan->mfault->SetFaultState(1, 0, "sendmsg nlen err");
+        }
+        sendmsg.nLen = nlen;
+        if(sendmsg.nLen > 0)
+        {
+            memcpy(sendmsg.data,frame.data.data(),sendmsg.nLen);
+        }
+
+        psendmsgvector->push_back(sendmsg);
+    }
+    pMutex->unlock();
+}
+

src/ros2/AutoWare2025/src/adc_can_nvidia_agx/src/nvcan.cpp

@@ -0,0 +1,349 @@
+#include "adc_can_nvidia_agx/nvcan.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <string.h>
+#include <signal.h>
+#include <ctype.h>
+#include <libgen.h>
+#include <time.h>
+#include <errno.h>
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <sys/uio.h>
+#include <net/if.h>
+
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+#include <QDateTime>
+
+#include <iostream>
+
+/* for hardware timestamps - since Linux 2.6.30 */
+#ifndef SO_TIMESTAMPING
+#define SO_TIMESTAMPING 37
+#endif
+
+/* from #include <linux/net_tstamp.h> - since Linux 2.6.30 */
+#define SOF_TIMESTAMPING_SOFTWARE (1<<4)
+#define SOF_TIMESTAMPING_RX_SOFTWARE (1<<3)
+#define SOF_TIMESTAMPING_RAW_HARDWARE (1<<6)
+
+#define MAXSOCK 16    /* max. number of CAN interfaces given on the cmdline */
+#define MAXIFNAMES 30 /* size of receive name index to omit ioctls */
+#define MAXCOL 6      /* number of different colors for colorized output */
+#define ANYDEV "any"  /* name of interface to receive from any CAN interface */
+#define ANL "\r\n"    /* newline in ASC mode */
+
+#define SILENT_INI 42 /* detect user setting on commandline */
+#define SILENT_OFF 0  /* no silent mode */
+#define SILENT_ANI 1  /* silent mode with animation */
+#define SILENT_ON  2  /* silent mode (completely silent) */
+
+#include <QTime>
+
+#define BUF_SIZE 1000
+
+std::string CANNAME[] = {"can0","can1"};
+
+nvcan::nvcan()
+{
+
+}
+
+void nvcan::run()
+{
+
+    int currmax = 2;
+    fd_set rdfs;
+    int s[MAXSOCK];
+    int ret;
+
+    int canfd_on = 1;
+    
+    struct sockaddr_can addr;
+    char ctrlmsg[CMSG_SPACE(sizeof(struct timeval) + 3*sizeof(struct timespec) + sizeof(__u32))];
+    struct iovec iov;
+    struct msghdr msg;
+    struct canfd_frame frame;
+    int nbytes, i, maxdlen;
+    (void)maxdlen;
+    struct ifreq ifr;
+    struct timeval tv, last_tv;
+    (void)tv;
+    (void)last_tv;
+    struct timeval timeout_config = { 0, 0 }, *timeout_current = 0;
+
+    
+    for(i=0;i<currmax;i++)
+    {
+        s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+        if (s[i] < 0) {
+            std::cout<<"Create Socket Error "<<std::endl;
+            return;
+        }
+
+        addr.can_family = AF_CAN;
+
+        memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+        strncpy(ifr.ifr_name, CANNAME[i].data(), 5);
+
+        if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
+            std::cout<<"SIOCGIFINDEX"<<std::endl;
+            return;
+        }
+        addr.can_ifindex = ifr.ifr_ifindex;
+
+        //CANFD Support
+        setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));
+
+        if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+            std::cout<<" bind error "<<std::endl;
+            return;
+        }
+    }
+
+    mbCANOpen = true;
+    std::cout<<"open can card successfully"<<std::endl;
+
+    iov.iov_base = &frame;
+    msg.msg_name = &addr;
+    msg.msg_iov = &iov;
+    msg.msg_iovlen = 1;
+    msg.msg_control = &ctrlmsg;
+
+    qint64 nLastRecv = QDateTime::currentMSecsSinceEpoch();
+    int nRecvState = 0; // 0 Have Data  1 No Data;
+
+    mbRunning = true;
+
+    int nrecvcount = 0;
+
+    while((mbthreadrun)&&(mbCANOpen))
+    {
+        FD_ZERO(&rdfs);
+        for (i=0; i<currmax; i++)
+            FD_SET(s[i], &rdfs);
+
+        if (timeout_current)
+            *timeout_current = timeout_config;
+
+        timeout_config.tv_sec= 0;
+        timeout_config.tv_usec = 100;;
+        timeout_current = &timeout_config;
+        ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, timeout_current);
+        if (ret < 0) {
+            std::cout<<"select error"<<std::endl;
+            mbCANOpen = false;
+            continue;
+        }
+
+        for (i=0; i<currmax; i++) {  /* check all CAN RAW sockets */
+
+            if (FD_ISSET(s[i], &rdfs)) {
+
+                nLastRecv = QDateTime::currentMSecsSinceEpoch();
+                /* these settings may be modified by recvmsg() */
+                iov.iov_len = sizeof(frame);
+                msg.msg_namelen = sizeof(addr);
+                msg.msg_controllen = sizeof(ctrlmsg);
+                msg.msg_flags = 0;
+
+                nbytes = recvmsg(s[i], &msg, 0);
+
+                if (nbytes < 0) {
+ //                   if ((errno == ENETDOWN) && !down_causes_exit) {
+                    if ((errno == ENETDOWN)) {
+                        std::cout<<"can card down"<<std::endl;
+                        fprintf(stderr, "%s: interface down\n", CANNAME[i].data());
+                        return;
+                    }
+                    continue;
+//                    perror("read");
+//                    return 1;
+                }
+
+                if ((size_t)nbytes == CAN_MTU)
+                    maxdlen = CAN_MAX_DLEN;
+                else if ((size_t)nbytes == CANFD_MTU)
+                    maxdlen = CANFD_MAX_DLEN;
+                else {
+                    continue;
+                }
+
+                nrecvcount++;
+  //              qDebug("receive msg.");
+                mMutex.lock();
+
+                basecan_msg msg;
+                msg.id = frame.can_id&0x1fffffff;
+                if((frame.can_id&0x80000000)!= 0)msg.isExtern = true;
+                else msg.isExtern = false;
+                if((frame.can_id&0x40000000)!= 0)msg.isRemote = true;
+                else msg.isRemote = false;
+                msg.nLen = frame.len;
+                msg.nRecvTime = std::chrono::system_clock::now().time_since_epoch().count();
+//                if(msg.id == 0x1c2)qDebug("id = %08x",msg.id);
+                if((frame.len<=64)&&(frame.len>0))memcpy(msg.data,frame.data,frame.len);
+                if(mMsgRecvBuf[i].size()<BUF_SIZE)
+                {
+                    mMsgRecvBuf[i].push_back(msg);
+                }
+
+                mMutex.unlock();
+
+            }
+        }
+
+        if((QDateTime::currentMSecsSinceEpoch() - nLastRecv)> 1000)
+        {
+            if(nRecvState == 0)
+            {
+                nRecvState = -1;
+            }
+        }
+        else
+        {
+            if(nRecvState == -1)
+            {
+                nRecvState = 0;
+            }
+        }
+
+        struct canfd_frame framesend[2500];
+
+
+        for(int nch =0;nch<currmax;nch++)
+        {
+            int nsend = 0;
+            mMutex.lock();
+            for(i=0;i<static_cast<int>( mMsgSendBuf[nch].size());i++)
+            {
+                if(i>=2500)break;
+                framesend[i].len = mMsgSendBuf[nch].at(i).nLen;
+                memcpy(framesend[i].data,mMsgSendBuf[nch].at(i).data,framesend[i].len);
+                framesend[i].can_id = mMsgSendBuf[nch].at(i).id;
+                if(mMsgSendBuf[nch].at(i).isExtern)
+                {
+                    framesend[i].can_id = framesend[i].can_id|0x80000000;
+                }
+                else
+                {
+                    framesend[i].can_id = framesend[i].can_id&0x7ff;
+                }
+                if(mMsgSendBuf[nch].at(i).isRemote)
+                {
+                    framesend[i].can_id= framesend[i].can_id|0x40000000;
+                }
+
+
+
+                nsend++;
+            }
+            mMsgSendBuf[nch].clear();
+            mMutex.unlock();
+            if(nsend > 0)
+            {
+                for(i=0;i<nsend;i++)
+                {
+                    int nsendbyte = 16;
+                    if(framesend[i].len>8)
+                    {
+                        nsendbyte = CANFD_MTU;
+                    }
+                if (write(s[nch], &framesend[i],nsendbyte) != nsendbyte) {
+                    perror("write error 1.");
+                    continue;
+                }
+                }
+            }
+
+        }
+
+    }
+
+    for (i=0; i<currmax; i++)
+    {
+        close(s[i]);
+    }
+    qDebug("nvcan thread close.");
+    mbRunning = false;
+}
+
+void nvcan::startdev()
+{
+    mbthreadrun = true;
+    mpthread = new std::thread(&nvcan::run,this);
+}
+
+void nvcan::stopdev()
+{
+    mbthreadrun = false;
+    mpthread->join();
+
+    int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    while(abs(nnow - nstart)<100)
+    {
+        nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(mbRunning == false)
+        {
+            qDebug("can is closed.");
+            break;
+        }
+    }
+}
+
+int nvcan::GetMessage(const int nch,basecan_msg *pMsg, const int nCap)
+{
+    if((nch>1)||(nch < 0))return -1;
+    if(mMsgRecvBuf[nch].size() == 0)return 0;
+
+    int nRtn;
+    nRtn = nCap;
+    mMutex.lock();
+    if(nRtn > static_cast<int>( mMsgRecvBuf[nch].size()))nRtn = mMsgRecvBuf[nch].size();
+    int i;
+    for(i=0;i<nRtn;i++)
+    {
+        memcpy(&pMsg[i],&(mMsgRecvBuf[nch].at(i)),sizeof(basecan_msg));
+    }
+
+    std::vector<basecan_msg>::iterator iter;
+    iter = mMsgRecvBuf[nch].begin();
+    for(i=0;i<nRtn;i++)
+    {
+        iter = mMsgRecvBuf[nch].erase(iter);
+    }
+
+    mMutex.unlock();
+
+    return nRtn;
+
+}
+
+int nvcan::SetMessage(const int nch, basecan_msg *pMsg)
+{
+    if((nch>1)||(nch < 0))return -1;
+
+    if(mMsgSendBuf[nch].size() > BUF_SIZE)return -2;
+
+    mMutex.lock();
+    mMsgSendBuf[nch].push_back(*pMsg);
+    mMutex.unlock();
+    return 0;
+}
+
+bool nvcan::IsOpen()
+{
+    return mbCANOpen;
+}
+
+

src/ros2/AutoWare2025/src/adc_chassis/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.8)
+project(adc_chassis)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(adc_autoware_msgs REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include/adc_chassis
+)
+link_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../lib
+)
+
+
+add_executable(${PROJECT_NAME}
+  src/adc_chassis_node.cpp
+  src/adc_chassis_core.cpp
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs 
+  adc_autoware_msgs)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ros2/AutoWare2025/src/adc_chassis/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/ros2/AutoWare2025/src/adc_chassis/include/adc_chassis/adc_chassis_core.h

@@ -0,0 +1,47 @@
+
+
+#ifndef ADC_CHASSIS_CORE_H_
+#define ADC_CHASSIS_CORE_H_
+
+
+#include <rclcpp/rclcpp.hpp>
+
+
+#include "adc_autoware_msgs/msg/adc_can_frame.hpp"
+#include "adc_autoware_msgs/msg/adc_can_msgs.hpp"
+#include "adc_autoware_msgs/msg/adc_chassis.hpp"
+
+
+class adc_chassis_core : public rclcpp::Node
+{
+public:
+    adc_chassis_core();
+    ~adc_chassis_core() = default;
+
+private:
+
+
+
+private:
+    std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+    std::string origin_frame_id_;     //!< @brief map frame_id
+    
+
+private:
+    
+    rclcpp::Publisher<adc_autoware_msgs::msg::AdcChassis>::SharedPtr pub_msgs_chassis;
+
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_msgs_canrecv0;
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr sub_msgs_canrecv1;
+
+private:
+    void topic_canrecv0_callback(const adc_autoware_msgs::msg::AdcCanMsgs & msg) const;
+    void topic_canrecv1_callback(const adc_autoware_msgs::msg::AdcCanMsgs & msg) const;
+
+
+
+};
+
+
+
+#endif

src/ros2/AutoWare2025/src/adc_chassis/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>adc_chassis</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>adc_autoware_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/AutoWare2025/src/adc_chassis/src/adc_chassis_core.cpp

@@ -0,0 +1,78 @@
+#include "adc_chassis/adc_chassis_core.h"
+
+#include <functional>
+
+using  std::placeholders::_1;
+
+adc_chassis_core::adc_chassis_core(): Node("adc_chassis")
+{
+    sub_msgs_canrecv0 = this->create_subscription<adc_autoware_msgs::msg::AdcCanMsgs>("canrecv0",10,
+                            std::bind(&adc_chassis_core::topic_canrecv0_callback,this,_1));
+    sub_msgs_canrecv1 = this->create_subscription<adc_autoware_msgs::msg::AdcCanMsgs>("canrecv1",10,
+                            std::bind(&adc_chassis_core::topic_canrecv1_callback,this,_1));
+    pub_msgs_chassis = this->create_publisher<adc_autoware_msgs::msg::AdcChassis>("chassis",10);
+}
+
+
+void adc_chassis_core::topic_canrecv0_callback(const adc_autoware_msgs::msg::AdcCanMsgs & msg) const
+{
+    int i;
+    int ncount = static_cast<int>(msg.frames.size());
+    static bool bhavevel = false;
+    static bool bhaveang = false;
+    static double vehspeed = 0;
+    static double ang = 0;
+
+ //   std::cout<<" size : "<<msg.frames.size()<<std::endl;
+    
+    for(i=0;i<ncount;i++)
+    {
+        unsigned char bytedata[64];
+
+//        std::cout<<" frame id:"<<msg.frames[i].id<<std::endl;
+
+        if(msg.frames[i].id == 0x1CC)
+        {
+            memcpy(bytedata,msg.frames[i].data.data(),64);
+            unsigned int value;
+            value = bytedata[12]&0x1F;
+            value = (value<<8) +  static_cast<unsigned int >( bytedata[13]);
+            vehspeed = static_cast<double>(value) * 0.05625;
+            bhavevel =  true;
+        }
+        if(msg.frames[i].id == 0x18a)
+        {
+            memcpy(bytedata,msg.frames[i].data.data(),64);
+            unsigned int value;
+            value = bytedata[0]&0xFF;
+            value = (value<<8) +  static_cast<unsigned int >( bytedata[1]);
+            if(value<32767)
+                ang = static_cast<double>(value) * 0.1;
+            else
+                ang=static_cast<double>(value) * 0.1-65536*0.1;
+            bhaveang = true;
+        }
+    }
+
+    if(bhaveang && bhavevel)
+    {
+        auto msgchassis = adc_autoware_msgs::msg::AdcChassis();
+        int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+        msgchassis.header.frame_id = "adc";
+        msgchassis.header.stamp.sec = nnow/1e9;
+        msgchassis.header.stamp.nanosec = nnow - msgchassis.header.stamp.sec * 1e9;
+        msgchassis.vel = vehspeed;
+        msgchassis.angle_feedback = ang;
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" vel: "<<msgchassis.vel<<" ang: "<<msgchassis.angle_feedback<<std::endl;
+        pub_msgs_chassis->publish(msgchassis);
+        bhaveang = false;
+        bhavevel = false;
+    }
+
+    (void)msg;
+}
+
+void adc_chassis_core::topic_canrecv1_callback(const adc_autoware_msgs::msg::AdcCanMsgs & msg) const
+{
+    (void)msg;
+}

src/ros2/AutoWare2025/src/adc_chassis/src/adc_chassis_node.cpp

@@ -0,0 +1,16 @@
+
+
+#include <rclcpp/rclcpp.hpp>
+#include "adc_chassis/adc_chassis_core.h"
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<adc_chassis_core>());
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.8)
+project(adc_controller_shenlan_v2)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(adc_autoware_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+find_package(Protobuf REQUIRED)
+#find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  include
+  include/adc_controller_shenlan_v2
+)
+
+#message(ERROR "catkin_INCLUDE_DIRS: ${catkin_INCLUDE_DIRS}")
+#message(STATUS "CMAKE_CURRENT_BINARY_DIR: ${CMAKE_CURRENT_BINARY_DIR}")
+
+add_executable(${PROJECT_NAME}
+  src/adc_controller_shenlan_v2_node.cpp
+  src/adc_controller_shenlan_v2_core.cpp
+  src/control_status.cpp
+  src/controller.cpp
+  src/decition.pb.cc
+)
+
+target_link_libraries(${PROJECT_NAME}  ${Protobuf_LIBRARIES})  
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs 
+  adc_autoware_msgs autoware_control_msgs )
+
+install(TARGETS
+  ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/adc_controller_shenlan_v2_core.hpp

@@ -0,0 +1,90 @@
+
+#ifndef ADC_CONTROLLER_SHENLAN_V2_CORE_HPP_
+#define ADC_CONTROLLER_SHENLAN_V2_CORE_HPP_
+
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <thread>
+#include <mutex>
+
+
+#include "adc_autoware_msgs/msg/adc_can_frame.hpp"
+#include "adc_autoware_msgs/msg/adc_can_msgs.hpp"
+#include "adc_autoware_msgs/msg/adc_chassis.hpp"
+#include "autoware_control_msgs/msg/control.hpp"
+#include "adc_autoware_msgs/srv/chassis_enable_ctrl.hpp"
+
+#include "decition.pb.h"
+
+
+class adc_controller_shenlan_v2_core : public rclcpp::Node
+{
+public:
+    adc_controller_shenlan_v2_core();
+    ~adc_controller_shenlan_v2_core();
+
+private:
+
+
+
+private:
+    std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+    std::string origin_frame_id_;     //!< @brief map frame_id
+    
+
+private:
+    
+    rclcpp::Subscription<adc_autoware_msgs::msg::AdcChassis>::SharedPtr sub_msgs_chassis;
+
+    rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_cansend0;
+
+    rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr sub_msgs_cmd;
+
+    rclcpp::Service<adc_autoware_msgs::srv::ChassisEnableCtrl>::SharedPtr srv_chassisenable;
+
+private:
+    void topic_chassis_callback(const adc_autoware_msgs::msg::AdcChassis & msg);
+    void topic_cmd_callback(const autoware_control_msgs::msg::Control & msg) ;
+
+    void service_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
+          std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response);
+
+private:
+    void threadsend();
+    std::thread * mpthreadsend;
+    bool mbtheadrun = true;
+
+private:
+    void executeDecition(const iv::brain::decition &decition);
+    void ExecSend();
+    void Activate();
+    void UnAcitvate();
+    void initial();
+
+private:
+    bool gbHaveVehSpd = false;
+    double gfVehSpd = 0.0;
+
+    bool gbHaveWheelAngle = false;
+    double gfWheelAngle = 0.0;
+
+    iv::brain::decition gdecition_def;
+    iv::brain::decition gdecition;
+
+    int gnLastSendTime = 0;
+    int gnLastRecvDecTime = -1000;
+    int gnDecitionNum = 0; //when is zero,send default;
+    int gnDecitionNumMax = 100;
+    int gnIndex = 0;
+
+    double gfWheelSpeedLim = 300; //300 degrees per second
+    std::mutex gMutex;
+
+    bool mbEnableChassis = true;
+
+
+
+};
+
+#endif

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/control_status.h

@@ -0,0 +1,108 @@
+#pragma once
+//由于控制器指令共享同一个ID 0x20，建立此类维护控制指令的最新状态
+
+//#include <boost.h>
+#include <cstdint>
+#include <boost/serialization/singleton.hpp>
+#include <adc_controller_shenlan_v2/vv7.h>
+
+namespace iv {
+namespace control {
+class ControlStatus : public boost::noncopyable
+{
+public:
+    /*****************
+             * ****测试标志位*****
+             * ***************/
+    int normal_speed  = 0;//常规速度
+    int swerve_speed  = 0;//转弯速度
+    int high_speed = 0;//快速
+    int mid_speed = 0;//中速
+    int low_speed = 0;//慢速
+    int change_line  = -1;//换道标志
+    int stop_obstacle = -1;//停障标志
+    int elude_obstacle = -1;//避障标志
+    int special_signle = -1;//特殊信号标志
+    int car_pullover = -1;//靠边停车标志位
+    float torque = 0.0;
+    float brake = 0.0;
+    float acc=0.0;
+
+    Command10 command10 ;
+    Command11 command11 ;
+    Command12 command12;
+    Command_Response command_reponse;
+
+    int command_ID10 = 0x10;
+    int command_ID11 = 0x11;
+    int command_ID12 = 0x12;
+
+
+    void set_wheel_angle(float angle);
+    void set_wheel_speed(float speed);
+    void set_wheel_enable(bool enable);
+
+
+
+
+
+    void set_speed_limit(float speed);
+    void set_torque(float percent);
+    void set_aeb(float aeb);
+    void set_brake(float brake);
+    void set_gear(int gear);
+    void set_handBrake(bool handBrake);
+    void set_driveMode(char mode);
+    void set_gear_enable(bool enable);
+    void set_aeb_enable(bool enable);
+    void set_acc_enable(bool enable);
+
+
+
+
+    void set_win_lf(char para);
+    void set_win_rf(char para);
+    void set_win_lr(char para);
+    void set_win_rr(char para);
+    void set_air_on(char para);
+    void set_air_cricle(char para);
+    void set_air_auto(char para);
+    void set_air_off(char para);
+    void set_air_temup(char para);
+    void set_air_temdown(char para);
+    void set_air_powerup(char para);
+    void set_air_powerdown(char para);
+    void set_obligate(char para);
+    void set_door(char enable);
+    void set_turnsignals_control(bool left, bool right);
+    void set_small_light(char para);
+    void set_near_light(char para);
+    void set_horn(char para);
+    void set_far_light(char para);
+    void set_frog_light(char para);
+    void set_wiper(char para);
+    void set_brake_light(char para);
+    void set_defrog(char para);
+    void set_reverse_light(char para);
+
+    void set_air_temp(char para);
+    void set_air_mode(char para);
+    void set_air_enable(bool enable);
+    void set_wind_level(char para);
+    void set_roof_light(char para);
+    void set_home_light(char para);
+    void set_air_worktime(char para);
+    void set_air_offtime(char para);
+
+    void set_cmd_checksum(unsigned char cmd_id);
+
+
+
+
+
+
+};
+typedef boost::serialization::singleton<iv::control::ControlStatus> ControlStatusSingleton;
+}
+}
+#define ServiceControlStatus iv::control::ControlStatusSingleton::get_mutable_instance()

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/controlcan.h

@@ -0,0 +1,145 @@
+#ifndef CONTROLCAN1_H
+#define CONTROLCAN1_H
+
+
+#ifdef linux
+//接口卡类型定义
+#define VCI_PCI5121		1
+#define VCI_PCI9810		2
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_PCI9820		5
+#define VCI_CAN232		6
+#define VCI_PCI5110		7
+#define VCI_CANLite		8
+#define VCI_ISA9620		9
+#define VCI_ISA5420		10
+
+//CAN错误码
+#define	ERR_CAN_OVERFLOW			0x0001	//CAN控制器内部FIFO溢出
+#define	ERR_CAN_ERRALARM			0x0002	//CAN控制器错误报警
+#define	ERR_CAN_PASSIVE				0x0004	//CAN控制器消极错误
+#define	ERR_CAN_LOSE				0x0008	//CAN控制器仲裁丢失
+#define	ERR_CAN_BUSERR				0x0010	//CAN控制器总线错误
+
+//通用错误码
+#define	ERR_DEVICEOPENED			0x0100	//设备已经打开
+#define	ERR_DEVICEOPEN				0x0200	//打开设备错误
+#define	ERR_DEVICENOTOPEN			0x0400	//设备没有打开
+#define	ERR_BUFFEROVERFLOW			0x0800	//缓冲区溢出
+#define	ERR_DEVICENOTEXIST			0x1000	//此设备不存在
+#define	ERR_LOADKERNELDLL			0x2000	//装载动态库失败
+#define ERR_CMDFAILED				0x4000	//执行命令失败错误码
+#define	ERR_BUFFERCREATE			0x8000	//内存不足
+
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+	
+#define USHORT unsigned short int
+#define BYTE unsigned char
+#define CHAR char
+#define UCHAR unsigned char
+#define UINT unsigned int
+#define DWORD unsigned int
+#define PVOID void*
+#define ULONG unsigned int
+#define INT int
+#define UINT32 UINT
+#define LPVOID void*
+#define BOOL BYTE
+#define TRUE 1
+#define FALSE 0
+
+
+#if 1
+
+typedef  struct  _VCI_BOARD_INFO{//1.ZLGCAN系列接口卡信息的数据类型。
+		USHORT	hw_Version;
+		USHORT	fw_Version;
+		USHORT	dr_Version;
+		USHORT	in_Version;
+		USHORT	irq_Num;
+		BYTE	can_Num;
+		CHAR	str_Serial_Num[20];
+		CHAR	str_hw_Type[40];
+		USHORT	Reserved[4];
+} VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
+
+
+typedef  struct  _VCI_CAN_OBJ{//2.定义CAN信息帧的数据类型。
+	UINT	ID;
+	UINT	TimeStamp;
+	BYTE	TimeFlag;
+	BYTE	SendType;
+	BYTE	RemoteFlag;//是否是远程帧
+	BYTE	ExternFlag;//是否是扩展帧
+	BYTE	DataLen;
+	BYTE	Data[8];
+	BYTE	Reserved[3];
+}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
+
+
+typedef struct _VCI_CAN_STATUS{//3.定义CAN控制器状态的数据类型。
+	UCHAR	ErrInterrupt;
+	UCHAR	regMode;
+	UCHAR	regStatus;
+	UCHAR	regALCapture;
+	UCHAR	regECCapture; 
+	UCHAR	regEWLimit;
+	UCHAR	regRECounter; 
+	UCHAR	regTECounter;
+	DWORD	Reserved;
+}VCI_CAN_STATUS,*PVCI_CAN_STATUS;
+
+
+typedef struct _ERR_INFO{//4.定义错误信息的数据类型。
+		UINT	ErrCode;
+		BYTE	Passive_ErrData[3];
+		BYTE	ArLost_ErrData;
+} VCI_ERR_INFO,*PVCI_ERR_INFO;
+
+
+typedef struct _INIT_CONFIG{//5.定义初始化CAN的数据类型
+	DWORD	AccCode;
+	DWORD	AccMask;
+	DWORD	Reserved;
+	UCHAR	Filter;
+	UCHAR	Timing0;	
+	UCHAR	Timing1;	
+	UCHAR	Mode;
+}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
+DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
+DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
+
+DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
+DWORD VCI_ReadErrInfo(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_ERR_INFO pErrInfo);
+DWORD VCI_ReadCANStatus(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_STATUS pCANStatus);
+
+DWORD VCI_GetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+
+ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
+ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+#endif
+
+#endif

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/controller.h

@@ -0,0 +1,83 @@
+#pragma once
+/*
+*控制器
+*/
+#ifndef _IV_CONTROL_CONTROLLER_
+#define _IV_CONTROL_CONTROLLER_
+
+//#include <boost.h>
+//#include <common/car_status.h>
+#include <adc_controller_shenlan_v2/control_status.h>
+
+namespace iv {
+    namespace control {
+        class Controller
+        {
+        public:
+            Controller();
+            ~Controller();
+            void inialize();// 初始化
+
+
+            void control_wheel(float angle);		//方向盘控制
+            void control_angle_speed(float angSpeed);
+            void control_angle_enable(bool enable);
+
+            void control_speed_limit(float speedLimit);
+            void control_torque(float percent);	//油门开度控制
+            void control_aeb(float aeb);	//油门开度控制
+            void control_brake(float brake);
+            void control_gear(float gear);
+            void control_handBrake(bool  enable);
+            void control_mode(char mode);
+            void control_gear_en(bool enable);
+            void control_aeb_en(bool enable);
+            void control_acc_en(bool enanble);
+
+
+            void control_win_lf(char para);
+            void control_win_rf(char para);
+            void control_win_lr(char para);
+            void control_win_rr(char para);
+            void control_air_on(bool enable);
+            void control_air_cricle(char para);
+            void control_air_auto(char para);
+            void control_air_off(char para);
+            void control_air_temup(char para);
+            void control_air_temdown(char para);
+            void control_air_powerup(char para);
+            void control_air_powerdown(char para);
+            void control_obligate(char para);
+            void control_door(char enable);
+            void control_turnsignals(bool left, bool right);
+            void control_small_light(char para);
+            void control_near_light(char para);
+            void control_horn(char para);
+            void control_far_light(char para);
+            void control_frog_light(char para);
+            void control_wiper(char para);
+            void control_brake_light(char para);
+            void control_defrog(char para);
+            void control_reverse_light(char para);
+            void cmd_checksum(unsigned char cmd_id);
+
+
+
+            void control_air_temp(char para);
+            void control_air_mode(char para);
+            void control_air_enable(bool enable);
+            void control_wind_level(char para);
+            void control_roof_light(char para);
+            void control_home_light(char para);
+            void control_air_worktime(char para);
+            void control_air_offtime(char para);
+
+            ///* 获取当前车辆状态*/
+            //void getCurrentCarStatus(iv::CarStatus & car_status);
+
+        private:
+        };
+    }
+}
+
+#endif // !_IV_CONTROL_CONTROLLER_

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/vv7.h

@@ -0,0 +1,114 @@
+#pragma once
+
+#if 1
+struct Command_Bit10
+{
+    unsigned char air_temp ;
+    unsigned char air_mode : 3;
+    unsigned char air_enable : 1;
+    unsigned char air_wind_level : 3;
+    unsigned char air_reserved : 1;
+    unsigned char ignition : 2;
+    unsigned char door : 2;
+    unsigned char Reserved :2;
+    unsigned char air_on : 2;
+    unsigned char turn_light : 2;
+    unsigned char small_light : 1;
+    unsigned char near_light : 1;
+    unsigned char horn : 1;
+    unsigned char far_light : 1;
+    unsigned char fog_light : 1;
+    unsigned char wiper : 1;
+    unsigned char brake_light : 1;
+    unsigned char defrog : 1;
+    unsigned char revers_light : 1;
+    unsigned char roof_light : 1;
+    unsigned char home_light : 1;
+    unsigned char Reserved0 :3;
+    unsigned char air_work_time;
+    unsigned char  air_off_time;
+    unsigned char  checkSum;
+};
+
+
+union Command10
+{
+    Command_Bit10 bit;
+    unsigned char byte[8];
+};
+
+
+struct Command_Bit11
+{
+    unsigned char speed_limit;
+    unsigned char  aeb_H;
+    unsigned char  aeb_L;
+    unsigned char torque;
+    unsigned char brake;
+    unsigned char gear:3;
+    unsigned char Reserved :1;
+    unsigned char park :2;
+    unsigned char Reserved0 :2;
+    unsigned char Reserved1 :2;
+    unsigned char Reserved2 :2;
+    unsigned char driveMode :1;
+    unsigned char gear_enable :1;
+    unsigned char aeb_enable :1;
+    unsigned char acc_enable :1;
+    unsigned char checkSum;
+};
+
+
+
+
+union Command11
+{
+    Command_Bit11 bit;
+    unsigned char byte[8];
+};
+
+
+struct Command_Bit12
+{
+    unsigned char ang_speed;
+    unsigned char  ang_H;
+    unsigned char  ang_L;
+    unsigned char reserved0;
+    unsigned char reserved1;
+    unsigned char reserved2;
+    unsigned char Reserved : 6;
+    unsigned char ang_enable : 1;
+    unsigned char Reserved0 : 1;
+    unsigned char checkSum;
+};
+
+
+union Command12
+{
+    Command_Bit12 bit;
+    unsigned char byte[8];
+};
+
+
+struct Command_Response_Bit
+{
+    unsigned char head;
+    unsigned char grade : 2;
+    unsigned char driveMode : 2;
+    unsigned char epb : 1;
+    unsigned char epsMode : 2;
+    unsigned char  obligate: 1;
+};
+
+union Command_Response
+{
+    Command_Response_Bit bit;
+    unsigned char byte[2];
+};
+
+#else
+
+#endif
+
+
+

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/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>adc_controller_shenlan_v2</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <depend>rclcpp</depend>
+  <depend>adc_autoware_msgs</depend>
+  <depend>autoware_control_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/src/adc_controller_shenlan_v2_core.cpp

@@ -0,0 +1,648 @@
+#include "adc_controller_shenlan_v2/adc_controller_shenlan_v2_core.hpp"
+
+#include <functional>
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+
+// signal: @ACC_LatAngReq    //更改CANid
+#define ECU_1C4_ACC_LatAngReq_CovFactor (0.1)
+// conversion value to CAN signal
+#define ECU_1C4_ACC_LatAngReq_toS(x) ((int16_t)((x) / 0.1 + 7200))
+
+// signal: @ACC_MotorTorqueMaxLimitRequest
+#define ECU_1C4_ACC_MotorTorqueMaxLimitRequest_CovFactor (0.02)
+// conversion value to CAN signal
+#define ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(x) ((int16_t)((x) / 0.02 + 1024))
+
+// signal: @ACC_MotorTorqueMinLimitRequest
+#define ECU_1C4_ACC_MotorTorqueMinLimitRequest_CovFactor (0.02)
+// conversion value to CAN signal
+#define ECU_1C4_ACC_MotorTorqueMinLimitRequest_toS(x) ((int16_t)((x) / 0.02 + 1024))
+
+typedef struct
+{
+    int16_t ACC_LatAngReq;
+    //uint8_t ADS_Reqmode;  //20221102， 新车没有此信号
+    int16_t ACC_MotorTorqueMaxLimitRequest;
+    uint8_t ACC_LatAngReqActive;
+    int16_t ACC_MotorTorqueMinLimitRequest;
+    //uint8_t ACC_ADCReqType; //20221102， 新车没有此信号
+} ECU_1C4_t;
+
+// signal: @ACC_AccTrqReq
+#define ECU_24E_ACC_AccTrqReq_CovFactor (1)
+// conversion value to CAN signal
+#define ECU_24E_ACC_AccTrqReq_toS(x) ((int16_t)((x) + 16384))
+
+// signal: @ACC_ACCTargetAcceleration
+#define ECU_24E_ACC_ACCTargetAcceleration_CovFactor (0.05)
+// conversion value to CAN signal
+#define ECU_24E_ACC_ACCTargetAcceleration_toS(x) ((int16_t)((x) / 0.05 + 100))
+
+// signal: @ACC_AEBTargetDeceleration
+#define ECU_24E_ACC_AEBTargetDeceleration_CovFactor (0.0005)
+// conversion value to CAN signal
+#define ECU_24E_ACC_AEBTargetDeceleration_toS(x) ((int32_t)((x) / 0.0005 + 32000))
+
+
+static int gnState = 0; //0 not act  1 act
+typedef struct
+{
+    int16_t ACC_AccTrqReq;
+    uint8_t ACC_AccTrqReqActive;
+    int16_t ACC_ACCTargetAcceleration;
+    int32_t ACC_AEBTargetDeceleration;
+    uint8_t ACC_AEBVehilceHoldReq;
+    uint8_t ADCReqMode;
+    uint8_t ACC_AEBActive;
+    uint8_t ACC_Driveoff_Request;
+    uint8_t ACC_DecToStop;
+    uint8_t ACC_CDDActive;
+    uint8_t ACC_ACCMode;
+} ECU_24E_t;
+
+typedef struct
+{
+    uint8_t CdcDoor;
+    uint8_t res1;
+    uint8_t res2;
+    uint8_t ADS_UDLCTurnLightReq;
+} ECU_25E_t;  //zhuanxiangdeng IDgenghuan
+
+unsigned char byte_1C4[64];//byte_144[8];
+unsigned char byte_24E[64];
+unsigned char byte_25E[32];
+
+ECU_1C4_t _m1C4 = {0,0,0,0};
+ECU_24E_t _m24E = {0,0,0,0,0,0,0,0,0,0,0};
+ECU_25E_t _m25E = {0,0,0,0};
+
+
+void service1_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
+          std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response)
+{
+    response->feedback_enable = request->benable;
+}
+
+adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_controller_shenlan_v2")
+{
+    gdecition_def.set_brake(0);
+    gdecition_def.set_rightlamp(false);
+    gdecition_def.set_leftlamp(false);
+    gdecition_def.set_wheelangle(0);
+
+    gdecition_def.set_angle_mode(0);
+    gdecition_def.set_angle_active(0);
+    gdecition_def.set_acc_active(0);
+//    gdecition_def.set_brake_active(1);
+    gdecition_def.set_brake_type(0);
+    gdecition_def.set_auto_mode(0); 
+
+    sub_msgs_chassis = this->create_subscription<adc_autoware_msgs::msg::AdcChassis>("chassis",10,
+                        std::bind(&adc_controller_shenlan_v2_core::topic_chassis_callback,this,_1));
+
+    pub_msgs_cansend0 = this->create_publisher<adc_autoware_msgs::msg::AdcCanMsgs>("cansend0",10);
+
+    sub_msgs_cmd = this->create_subscription<autoware_control_msgs::msg::Control>("xx",10,
+                        std::bind(&adc_controller_shenlan_v2_core::topic_cmd_callback,this,_1));
+
+    srv_chassisenable = this->create_service<adc_autoware_msgs::srv::ChassisEnableCtrl>("/adc_controller_shenlan_v2/chassisenable",
+                        std::bind(&adc_controller_shenlan_v2_core::service_enablechassis,this,_1,_2));
+
+    mpthreadsend = new std::thread(&adc_controller_shenlan_v2_core::threadsend,this);
+}
+
+adc_controller_shenlan_v2_core::~adc_controller_shenlan_v2_core()
+{
+    mbtheadrun = false;
+    mpthreadsend->join();
+}
+
+void adc_controller_shenlan_v2_core::topic_chassis_callback(const adc_autoware_msgs::msg::AdcChassis & msg)
+{
+    (void)msg;
+
+    gfVehSpd = msg.vel;
+    gbHaveVehSpd = true;
+
+
+
+    gfWheelAngle = msg.angle_feedback;
+    gbHaveWheelAngle = true;
+
+}
+
+void adc_controller_shenlan_v2_core::threadsend()
+{
+    initial();
+    iv::brain::decition xdecition;
+
+    UnAcitvate();
+ //   UnAcitvate();
+
+    int nstate = 0; //0 Un 1 Activate
+//    Activate();
+    while(mbtheadrun)
+    {
+        if(gnDecitionNum <= 0)
+        {
+            if(nstate == 1)
+            {
+                UnAcitvate();
+                nstate = 0;
+            }
+            xdecition.CopyFrom(gdecition_def);
+        }
+        else
+        {
+            if(mbEnableChassis)
+            {
+                if(nstate == 0)
+                {
+                    Activate();
+                    nstate = 1;
+                }
+            }
+            else
+            {
+                if(nstate == 1)
+                {
+                    UnAcitvate();
+                    nstate = 0;
+                }
+            }
+            gMutex.lock();
+            xdecition.CopyFrom(gdecition);
+            gMutex.unlock();
+            gnDecitionNum--;
+        }
+
+        executeDecition(xdecition);
+        ExecSend();
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+//    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" before unactivate."<<std::endl;
+//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    UnAcitvate();
+
+//    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" send unactivate msg to can. Wait CAN Excute Send."<<std::endl;
+//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" thread send exit."<<std::endl;
+}
+
+void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_control_msgs::msg::Control & msg)
+{
+    (void)msg;
+        iv::brain::decition xdecition;
+    double facc = 0;
+    double ftorque = 0;
+    double fbrake = 0;
+    double fwheelangle = 0;
+
+    facc = msg.longitudinal.acceleration;
+
+        double fVehWeight = 1800;
+     //   double fg = 9.8;
+        double fRollForce = 50;
+        const double fRatio = 2.5;
+        double fNeed = fRollForce + fVehWeight*facc;
+
+        ftorque = fNeed/fRatio;
+        fbrake = 0;
+        if(ftorque<0)
+        {
+            fbrake = facc;
+            ftorque = 0;
+        }
+
+//    facc = xctrlcmd.linear_acceleration();
+    fwheelangle = msg.lateral.steering_tire_angle * (180.0/M_PI) * 15.0;
+    if(fwheelangle>430)fwheelangle = 430;
+    if(fwheelangle<-430)fwheelangle = -430;
+    std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
+    xdecition.set_accelerator(facc);
+    xdecition.set_brake(fbrake);
+    xdecition.set_leftlamp(false);
+    xdecition.set_rightlamp(false);
+    xdecition.set_speed(msg.longitudinal.velocity * 3.6);
+    xdecition.set_wheelangle(fwheelangle);
+    xdecition.set_wheelspeed(300);
+    xdecition.set_torque(ftorque);
+    xdecition.set_mode(1);
+    xdecition.set_gear(1);
+    xdecition.set_handbrake(0);
+    xdecition.set_grade(1);
+    xdecition.set_engine(0);
+    xdecition.set_brakelamp(false);
+    xdecition.set_ttc(15);
+//    xdecition.set_air_enable(decition->air_enable);
+//    xdecition.set_air_temp(decition->air_temp);
+//    xdecition.set_air_mode(decition->air_mode);
+//    xdecition.set_wind_level(decition->wind_level);
+    xdecition.set_roof_light(0);
+    xdecition.set_home_light(0);
+//    xdecition.set_air_worktime(decition->air_worktime);
+//    xdecition.set_air_offtime(decition->air_offtime);
+//    xdecition.set_air_on(decition->air_on);
+    xdecition.set_door(0);
+    xdecition.set_dippedlight(0);
+
+    gMutex.lock();
+    gdecition.CopyFrom(xdecition);
+    gMutex.unlock();
+
+    gnDecitionNum = gnDecitionNumMax;
+}
+
+void adc_controller_shenlan_v2_core::executeDecition(const iv::brain::decition &decition)
+{
+
+
+
+
+    static int xieya = 50;
+//     std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
+//     std::cout<<"angle_mode is "<<decition.angle_mode()<<" angle_active is "<<decition.angle_active()<<std::endl;
+//     std::cout<<"brake_type is "<<decition.brake_type()<<" acc_active is "<<decition.acc_active()<<std::endl;
+//     std::cout<<"brake is "<<decition.brake()<<" brake_active is "<<decition.brake_active()<<std::endl;
+//     std::cout<<"auto_mode is "<<decition.auto_mode()<<" rightlamp is "<<decition.rightlamp()<<std::endl;
+
+    double fWheelAngleReq = decition.wheelangle();
+ //   double fsendinter = 0.02;
+//    if(fabs(fWheelAngleReq - gfWheelAngle)/fsendinter > gfWheelSpeedLim)
+//    {
+//        if(fWheelAngleReq > gfWheelAngle)
+//        {
+//            fWheelAngleReq = gfWheelAngle + fsendinter * gfWheelSpeedLim;
+//        }
+//        else
+//        {
+//            fWheelAngleReq = gfWheelAngle - fsendinter * gfWheelSpeedLim;
+//        }
+
+//    }
+
+//    std::cout<<" wheel req: "<<decition.wheelangle()<<" real send : "<<fWheelAngleReq<<" real whhel:"<<gfWheelAngle<< std::endl;
+
+    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(fWheelAngleReq);
+    //_m1C4.ADS_Reqmode = decition.angle_mode(); //20221102,新车没有此信号
+    _m1C4.ACC_MotorTorqueMaxLimitRequest = ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(10);
+    _m1C4.ACC_LatAngReqActive = decition.angle_active();
+    _m1C4.ACC_MotorTorqueMinLimitRequest = ECU_1C4_ACC_MotorTorqueMinLimitRequest_toS(-10);
+//    _m144.ACC_ADCReqType = decition.brake_type();//ADC请求类型（制动时为1，其余情况为0）
+    if(decition.brake()>(-0.0000001))
+    {
+        //_m144.ACC_ADCReqType = 0;//ADC请求类型（制动时为1，其余情况为0）//20221102,新车没有此信号
+       // _m24B.ADCReqMode = 0;//20221102,新车没有此信号
+        _m24E.ACC_DecToStop =0;
+    }
+    else
+    {
+        //_m144.ACC_ADCReqType = 1;//20221102,新车没有此信号
+       // _m24B.ADCReqMode = 1;//20221102,新车没有此信号
+         _m24E.ACC_DecToStop =1;
+    }
+
+//    std::cout<<" type: "<<(int)_m144.ACC_ADCReqType<<std::endl;
+
+
+    /*制动过程用的减速度，加速用扭矩*/
+    _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(decition.torque());
+    _m24E.ACC_AccTrqReqActive = decition.acc_active();
+    if(decition.brake()<(-5.0))
+    {
+        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(-5.0);
+    }
+    else
+        _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(decition.brake());
+
+
+ //   std::cout<<" brake : "<<decition.brake()<<std::endl;
+ //   std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
+    if(decition.brake()>(-0.0000001))
+    {
+
+        if(xieya > 0)
+        {
+            _m24E.ACC_CDDActive = 1;
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0.5);
+            _m24E.ACC_AccTrqReq = ECU_24E_ACC_AccTrqReq_toS(0);
+            _m24E.ACC_Driveoff_Request = 1;
+
+            //_m144.ACC_ADCReqType = 1;//20221102,新车没有此信号
+            //_m24B.ADCReqMode = 1;   //20221102,新车没有此信号
+             _m24E.ACC_DecToStop =1;
+            xieya--;
+            std::cout<<" xieya. now veh speed: "<<gfVehSpd<<std::endl;
+        }
+        else
+        {
+            _m24E.ACC_CDDActive = 0;
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0);
+            _m24E.ACC_Driveoff_Request = 0;
+        }
+    }
+    else
+    {
+        _m24E.ACC_CDDActive = 1;
+
+        _m24E.ACC_Driveoff_Request = 0;
+
+
+    }
+
+
+ //       _m24B.ACC_CDDActive = decition.brake_active();
+
+
+
+//    std::cout<<" req mode: "<<_m144.ADS_Reqmode<<std::endl;
+    //byte_144[0] = ((_m144.ACC_LatAngReq >> 9) & (0x1FU)) | ((_m144.ADS_Reqmode & (0x07U)) << 5);
+ //   qDebug(" req mode: %d byte: %02X  ",_m144.ADS_Reqmode, byte_144[0]);
+   // byte_144[1] = ((_m144.ACC_LatAngReq >> 1) & (0xFFU));
+    //byte_144[2] = ((_m144.ACC_LatAngReq & (0x01U)) << 7) | ((_m144.ACC_MotorTorqueMaxLimitRequest >> 5) & (0x3FU)) | ((_m144.ACC_LatAngReqActive & (0x01U)) << 6);
+    //byte_144[3] = ((_m144.ACC_MotorTorqueMaxLimitRequest & (0x1FU)) << 3) | ((_m144.ACC_MotorTorqueMinLimitRequest >> 8) & (0x07U));
+    //byte_144[4] = (_m144.ACC_MotorTorqueMinLimitRequest & (0xFFU));
+    //byte_144[5] = ((_m144.ACC_ADCReqType & (0x03U)) << 6);
+
+
+    byte_1C4[0] = ((_m1C4.ACC_MotorTorqueMaxLimitRequest >> 3) & (0xFFU));
+ //   qDebug(" req mode: %d byte: %02X  ",_m144.ADS_Reqmode, byte_144[0]);
+    byte_1C4[1] = ((_m1C4.ACC_MotorTorqueMaxLimitRequest & (0x07U)) << 5) | ((_m1C4.ACC_MotorTorqueMinLimitRequest >> 6) & (0x1FU));
+    byte_1C4[2] = ((_m1C4.ACC_MotorTorqueMinLimitRequest & (0x3FU)) << 2) | ((_m1C4.ACC_LatAngReq >> 12) & (0x03U));
+    byte_1C4[3] = (((_m1C4.ACC_LatAngReq>>4 )& (0xFFU)));
+   // byte_1C4[4] = (((_m1C4.ACC_LatAngReq<<4) & (0xF0U)))|((_m1C4.ACC_LatAngReqActive&(0x01U))<<3);
+    byte_1C4[4] = (((_m1C4.ACC_LatAngReq)& (0x0fU))<<4)|((_m1C4.ACC_LatAngReqActive&(0x01U))<<3);
+    //byte_1C4[5] = ((_m144.ACC_ADCReqType & (0x03U)) << 6);
+
+    //    _m24B.ACC_AEBTargetDeceleration = 0;
+    //    _m24B.ACC_AEBVehilceHoldReq = 0;
+    //    _m24B.ADCReqMode = 0;
+    //    _m24B.ACC_AEBActive = 0;
+    //    _m24B.ACC_Driveoff_Request = 0;
+    //    _m24B.ACC_DecToStop = 0;
+//    std::cout<<" brake : "<<decition.brake()<<std::endl;
+//    std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
+    if(decition.brake()>(-0.0000001))
+    {
+        _m24E.ACC_CDDActive = 0;
+        _m24E.ACC_Driveoff_Request = 1;
+        if(xieya > 0)
+        {
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0.5);
+            xieya--;
+        }
+        else
+        {
+            _m24E.ACC_ACCTargetAcceleration = ECU_24E_ACC_ACCTargetAcceleration_toS(0);
+        }
+        _m24E.ACC_AccTrqReqActive = 1;
+        _m24E.ACC_DecToStop = 0;
+
+    }
+    else
+    {
+        _m24E.ACC_CDDActive = 1;
+
+        _m24E.ACC_Driveoff_Request = 0;
+
+        if(gbHaveVehSpd)
+        {
+            if(gfVehSpd < 0.01)
+            {
+                if(xieya != 50)std::cout<<"need xieya. "<<std::endl;
+                xieya = 50;
+
+            }
+        }
+
+        _m24E.ACC_AccTrqReqActive = 0;
+        _m24E.ACC_DecToStop = 1;
+    }
+ //       _m24B.ACC_CDDActive = decition.brake_active();
+    _m24E.ACC_ACCMode = decition.auto_mode();
+
+
+    if(gnState == 0)
+    {
+        _m24E.ACC_CDDActive = 0;
+        _m24E.ACC_Driveoff_Request = 0;
+        _m24E.ACC_ACCMode = 0;
+        _m24E.ACC_ACCTargetAcceleration = 0;
+        _m24E.ACC_AccTrqReqActive = 0;
+        _m24E.ACC_DecToStop = 0;
+
+    }
+//    std::cout<<"acc mode: "<<(int)_m24B.ACC_ACCMode<<std::endl;
+
+
+   // byte_24B[0] = ((_m24B.ACC_AccTrqReq >> 7) & (0xFFU));
+    //byte_24B[1] = ((_m24B.ACC_AccTrqReq & (0x7FU)) << 1) | (_m24B.ACC_AccTrqReqActive & (0x01U));
+    //byte_24B[2] = ((_m24B.ACC_ACCTargetAcceleration >> 3) & (0x1FU));
+    //byte_24B[3] = ((_m24B.ACC_ACCTargetAcceleration & (0x07U)) << 5) | ((_m24B.ACC_AEBTargetDeceleration >> 15) & (0x01U));
+    //byte_24B[4] = ((_m24B.ACC_AEBTargetDeceleration >> 7) & (0xFFU));
+    //byte_24B[5] = ((_m24B.ACC_AEBTargetDeceleration & (0x7FU)) << 1) | (_m24B.ACC_AEBVehilceHoldReq & (0x01U));
+    //byte_24B[6] = ((_m24B.ADCReqMode & (0x03U)) << 1) | ((_m24B.ACC_AEBActive & (0x01U)) << 3) | ((_m24B.ACC_Driveoff_Request & (0x01U)) << 5) | ((_m24B.ACC_DecToStop & (0x01U)) << 6) | ((_m24B.ACC_CDDActive & (0x01U)) << 7);
+    //byte_24B[7] = (_m24B.ACC_ACCMode & (0x07U));
+
+
+    byte_24E[0] = ((_m24E.ACC_ACCTargetAcceleration) & (0xFFU));
+    //byte_24E[1] = ((_m24B.ACC_AccTrqReq & (0x7FU)) << 1) | (_m24B.ACC_AccTrqReqActive & (0x01U));
+    //byte_24E[2] = ((_m24B.ACC_ACCTargetAcceleration >> 3) & (0x1FU));
+    byte_24E[3] = ((_m24E.ACC_DecToStop & (0x01U)) << 7);// | ((_m24B.ACC_AEBTargetDeceleration >> 15) & (0x01U));
+    //byte_24E[4] = ((_m24B.ACC_AEBTargetDeceleration >> 7) & (0xFFU));
+    byte_24E[5] = ((_m24E.ACC_CDDActive & (0x01U)) << 4);//((_m24E.ACC_AEBTargetDeceleration & (0x7FU)) << 1) | (_m24E.ACC_AEBVehilceHoldReq & (0x01U));
+    byte_24E[6] = ((_m24E.ACC_Driveoff_Request & (0x01U)) << 7)|((_m24E.ACC_ACCMode & (0x07U))<<4);//((_m24E.ADCReqMode & (0x03U)) << 1) | ((_m24E.ACC_AEBActive & (0x01U)) << 3) | ((_m24E.ACC_Driveoff_Request & (0x01U)) << 5) | ((_m24E.ACC_DecToStop & (0x01U)) << 6) | ((_m24E.ACC_CDDActive & (0x01U)) << 7);
+    byte_24E[8] = ((_m24E.ACC_AEBTargetDeceleration>>8) & (0xFFU));
+    byte_24E[9] = ((_m24E.ACC_AEBTargetDeceleration) & (0xFFU));
+    byte_24E[10] = ((_m24E.ACC_AEBActive & (0x01U)) << 7);
+    byte_24E[11] = ((_m24E.ACC_AccTrqReq>>13 )& (0x03U));
+    byte_24E[12] = ((_m24E.ACC_AccTrqReq>>5 )& (0xFFU));
+    //byte_24E[13] = ((_m24E.ACC_AccTrqReq & (0x1FU)<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
+    byte_24E[13] = (((_m24E.ACC_AccTrqReq & (0x1FU))<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
+
+    if(decition.leftlamp() == true && decition.rightlamp() == false)
+        _m25E.ADS_UDLCTurnLightReq = 3;
+    else if(decition.leftlamp() == false && decition.rightlamp() == true)
+        _m25E.ADS_UDLCTurnLightReq = 4;
+    else
+        _m25E.ADS_UDLCTurnLightReq = 0;
+
+    byte_25E[3] = ((_m25E.ADS_UDLCTurnLightReq & (0x07U)));
+
+}
+
+
+void adc_controller_shenlan_v2_core::ExecSend()
+{
+    static int nCount = 0;
+    nCount++;
+
+    auto xmsg = adc_autoware_msgs::msg::AdcCanMsgs();
+    auto xraw = adc_autoware_msgs::msg::AdcCanFrame();
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    xraw.header.frame_id = "adc";
+    xraw.header.stamp.sec = nnow/1e9;
+    xraw.header.stamp.nanosec = nnow - xraw.header.stamp.sec * 1e9;
+    xraw.id = 0x1C4;
+    memcpy(xraw.data.data(),byte_1C4,32);
+    xraw.dlc = 32;
+    xraw.is_error = false;
+    xraw.is_extended = false;
+    xraw.is_rtr = false;
+    xmsg.frames.push_back(xraw);
+
+    nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    xraw.header.frame_id = "adc";
+    xraw.header.stamp.sec = nnow/1e9;
+    xraw.header.stamp.nanosec = nnow - xraw.header.stamp.sec * 1e9;
+    xraw.id = 0x24E;
+    memcpy(xraw.data.data(),byte_24E,32);
+    xraw.dlc = 64;
+    xraw.is_error = false;
+    xraw.is_extended = false;
+    xraw.is_rtr = false;
+    xmsg.frames.push_back(xraw);
+    if(nCount%2 == 1)
+    {
+        xmsg.frames.push_back(xraw);
+    }
+
+    nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    xraw.header.frame_id = "adc";
+    xraw.header.stamp.sec = nnow/1e9;
+    xraw.header.stamp.nanosec = nnow - xraw.header.stamp.sec * 1e9;
+    xraw.id = 0x25E;
+    memcpy(xraw.data.data(),byte_25E,32);
+    xraw.dlc = 32;
+    xraw.is_error = false;
+    xraw.is_extended = false;
+    xraw.is_rtr = false;
+    xmsg.frames.push_back(xraw);
+    if(nCount%2 == 1)
+    {
+        xmsg.frames.push_back(xraw);
+    }
+
+
+    nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    xmsg.header.frame_id = "adc";
+    xmsg.header.stamp.sec = nnow/1e9;
+    xmsg.header.stamp.nanosec = nnow - xraw.header.stamp.sec * 1e9;
+
+ //   std::cout<<" pub msg. "<<std::endl;
+
+    pub_msgs_cansend0->publish(xmsg);
+
+}
+
+
+void adc_controller_shenlan_v2_core::Activate()
+{
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    iv::brain::decition xdecition;
+    xdecition.set_brake(0.0);
+    xdecition.set_torque(0.0);
+//    for(int j=0;j<100000;j++)
+//    {
+        std::cout<<" run "<<std::endl;
+    for(int i = 0; i < 3; i++){
+        xdecition.set_wheelangle(0.0);
+        xdecition.set_angle_mode(0);
+        xdecition.set_angle_active(0);
+        xdecition.set_acc_active(0);
+        xdecition.set_brake_active(0);
+//        xdecition.set_brake_type(0);
+        xdecition.set_auto_mode(0);
+        gnState = 0;
+        executeDecition(xdecition);
+        ExecSend();
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    for(int i = 0; i < 3; i++){
+        xdecition.set_wheelangle(0.0);
+        xdecition.set_angle_mode(1);
+        xdecition.set_angle_active(1);
+        xdecition.set_acc_active(1);
+        xdecition.set_brake_active(1);
+//        xdecition.set_brake_type(1);
+        xdecition.set_auto_mode(3);
+        gnState = 1;
+        executeDecition(xdecition);
+        ExecSend();
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+//    }
+}
+
+void adc_controller_shenlan_v2_core::UnAcitvate()
+{
+    iv::brain::decition xdecition;
+
+    xdecition.set_brake(0.0);
+    xdecition.set_torque(0.0);
+    for(int i = 0; i < 3; i++){
+        xdecition.set_wheelangle(0);
+        xdecition.set_angle_mode(1);
+        xdecition.set_angle_active(1);
+        xdecition.set_acc_active(1);
+        xdecition.set_brake_active(1);
+//        xdecition.set_brake_type(1);
+        xdecition.set_auto_mode(3);
+        gnState = 1;
+        executeDecition(xdecition);
+        ExecSend();
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+    for(int i = 0; i < 3; i++){
+        xdecition.set_wheelangle(0);
+        xdecition.set_angle_mode(0);
+        xdecition.set_angle_active(0);
+        xdecition.set_acc_active(0);
+        xdecition.set_brake_active(0);
+//        xdecition.set_brake_type(0);
+        gnState = 0;
+        xdecition.set_auto_mode(0);
+        executeDecition(xdecition);
+        ExecSend();
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+void adc_controller_shenlan_v2_core::initial()
+{
+    for (uint8_t i = 0; i < 64; i++) //CAN  to  canfd
+    {
+        byte_1C4[i] = 0;
+        byte_24E[i] = 0;
+        //byte_36E[i] = 0;
+    }
+}
+
+
+//ros2 service call /adc_controller_shenlan_v2/chassisenable adc_autoware_msgs/srv/ChassisEnableCtrl "{benable: true}"
+//ros2 service call /adc_controller_shenlan_v2/chassisenable adc_autoware_msgs/srv/ChassisEnableCtrl "{benable: false}"
+void adc_controller_shenlan_v2_core::service_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
+          std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response)
+{
+    response->feedback_enable = request->benable;
+    mbEnableChassis = request->benable;
+    if(mbEnableChassis)
+    {
+        std::cout<<"service enable chassis."<<std::endl;
+    }
+    else
+    {
+        std::cout<<"service disable chassis."<<std::endl;
+    }
+}
+
+
+
+
+

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/src/adc_controller_shenlan_v2_node.cpp

@@ -0,0 +1,21 @@
+
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+#include "adc_controller_shenlan_v2/adc_controller_shenlan_v2_core.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<adc_controller_shenlan_v2_core>());
+
+  std::cout<<" shut. "<<std::endl;
+  
+
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/src/control_status.cpp

@@ -0,0 +1,487 @@
+#include <adc_controller_shenlan_v2/control_status.h>
+
+
+
+
+
+//#if 1
+
+
+void iv::control::ControlStatus::set_cmd_checksum(unsigned char cmd_id)
+{
+    unsigned int check_sum=0;
+    if(cmd_id==0x10)
+    {
+        for(int i=0;i<7;i++)
+        {
+            check_sum^=(ServiceControlStatus.command10.byte[i]);
+        }
+        ServiceControlStatus.command10.byte[7]=check_sum;
+    }else if(cmd_id==0x11)
+    {
+        for(int i=0;i<7;i++)
+        {
+            check_sum^=(ServiceControlStatus.command11.byte[i]);
+        }
+        ServiceControlStatus.command11.byte[7]=check_sum;
+    }else if(cmd_id==0x12)
+    {
+        for(int i=0;i<7;i++)
+        {
+            check_sum^=(ServiceControlStatus.command12.byte[i]);
+        }
+        ServiceControlStatus.command12.byte[7]=check_sum;
+    }
+}
+
+
+
+
+void iv::control::ControlStatus::set_wheel_angle(float angle)
+{
+
+    int ang =(angle+870)*10;
+    ServiceControlStatus.command12.bit.ang_H = (ang)  /256;
+    ServiceControlStatus.command12.bit.ang_L = (ang)  % 256;
+}
+
+
+
+void iv::control::ControlStatus::set_wheel_speed(float speed)
+{
+
+    int angSpeed =speed;
+    ServiceControlStatus.command12.bit.ang_speed = angSpeed;
+}
+
+void iv::control::ControlStatus::set_wheel_enable(bool enable)
+{
+
+    if(enable){
+        ServiceControlStatus.command12.bit.ang_enable = 1;
+    }else{
+        ServiceControlStatus.command12.bit.ang_enable = 0;
+    }
+}
+
+
+
+
+ void iv::control::ControlStatus::set_speed_limit(float speed)
+ {
+     ServiceControlStatus.command11.bit.speed_limit = (unsigned)speed;
+ }
+
+
+
+void iv::control::ControlStatus::set_aeb(float aeb)
+{
+    int aebBrake =(aeb+16)/0.000488;
+    ServiceControlStatus.command11.bit.aeb_H = (aebBrake)  /256;
+    ServiceControlStatus.command11.bit.aeb_L = (aebBrake)  % 256;
+}
+
+void iv::control::ControlStatus::set_torque(float troque)
+{
+    command11.bit.torque=(unsigned)(troque*2.5);
+}
+
+void iv::control::ControlStatus::set_brake(float brake)
+{
+    command11.bit.brake=(unsigned)brake;
+}
+
+
+
+void iv::control::ControlStatus::set_gear(int gear)
+{
+    command11.bit.gear = (unsigned)gear;
+}
+
+void iv::control::ControlStatus::set_handBrake(bool handbrake)
+{
+    if(handbrake){
+        command11.bit.park = 2;
+    }else{
+        command11.bit.park = 1;
+    }
+}
+
+void iv::control::ControlStatus::set_driveMode(char driveMode)
+{
+    command11.bit.driveMode = (unsigned)driveMode;
+}
+
+void iv::control::ControlStatus::set_gear_enable(bool enable){
+    if (enable == true)
+        command11.bit.gear_enable = 1;
+    else
+        command11.bit.gear_enable = 0;
+}
+
+void iv::control::ControlStatus::set_acc_enable(bool enable){
+    if (enable == true)
+        command11.bit.acc_enable = 1;
+    else
+        command11.bit.acc_enable = 0;
+}
+
+void iv::control::ControlStatus::set_aeb_enable(bool enable){
+    if (enable == true)
+        command11.bit.aeb_enable = 1;
+    else
+        command11.bit.aeb_enable = 0;
+}
+
+
+//void iv::control::ControlStatus::set_win_lf(char para)
+//{
+//    command10.bit.windowlf = (unsigned)para;
+//}
+
+//void iv::control::ControlStatus::set_win_rf(char para)
+//{
+//    command10.bit.windowrf = (unsigned)para;
+//}
+
+//void iv::control::ControlStatus::set_win_lr(char para)
+//{
+//    command10.bit.windowlr = (unsigned)para;
+//}
+
+//void iv::control::ControlStatus::set_win_rr(char para)
+//{
+//    command10.bit.windowrr = (unsigned)para;
+//}
+
+//空调控制
+void iv::control::ControlStatus::set_air_on(char para)
+{
+    command10.bit.air_on = (unsigned)para;
+}
+
+
+
+//void iv::control::ControlStatus::set_air_cricle(char para)
+//{
+//    command10.bit.air_cricle = (unsigned)para;
+//}
+
+
+//void iv::control::ControlStatus::set_air_auto(char para)
+//{
+//    command10.bit.air_auto = (unsigned)para;
+//}
+
+
+//void iv::control::ControlStatus::set_air_off(char para)
+//{
+//    command10.bit.air_off = (unsigned)para;
+//}
+
+
+//void iv::control::ControlStatus::set_air_temup(char para)
+//{
+//    command10.bit.tem_up = (unsigned)para;
+//}
+
+
+//void iv::control::ControlStatus::set_air_temdown(char para)
+//{
+//    command10.bit.tem_down = (unsigned)para;
+//}
+
+
+//void iv::control::ControlStatus::set_air_powerup(char para)
+//{
+//    command10.bit.power_up = (unsigned)para;
+//}
+
+
+//void iv::control::ControlStatus::set_air_powerdown(char para)
+//{
+//    command10.bit.power_down = (unsigned)para;
+//}
+
+
+void iv::control::ControlStatus::set_obligate(char para)
+{
+    command10.bit.ignition = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_door(char enable)
+{
+    command10.bit.door = (unsigned)enable;
+}
+
+
+void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right)
+{
+    if ( (left == true) && (right == false) )
+    {
+        command10.bit.turn_light = 0x01;
+    }
+    else if ( (left == false) && (right == true) )
+    {
+        command10.bit.turn_light = 0x02;
+    }
+    else if ((left == false) && (right == false))
+    {
+        command10.bit.turn_light = 0x00;
+    }
+    else
+    {
+        command10.bit.turn_light = 0x03;
+    }
+}
+
+
+void iv::control::ControlStatus::set_small_light(char para)
+{
+    command10.bit.small_light = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_near_light(char para)
+{
+    command10.bit.near_light = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_horn(char para)
+{
+    command10.bit.horn = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_far_light(char para)
+{
+    command10.bit.far_light = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_frog_light(char para)
+{
+    command10.bit.fog_light = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_wiper(char para)
+{
+    command10.bit.wiper = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_brake_light(char para)
+{
+    command10.bit.brake_light = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_defrog(char para)
+{
+    command10.bit.defrog = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_reverse_light(char para)
+{
+    command10.bit.revers_light = (unsigned)para;
+}
+
+
+void set_air_temp(char para);
+void set_air_mode(char para);
+void set_air_enable(bool enable);
+void set_wind_level(char para);
+void set_roof_light(char para);
+void set_home_light(char para);
+void set_air_worktime(char para);
+void set_air_offtime(char para);
+
+void iv::control::ControlStatus::set_air_temp(char para)
+{
+    command10.bit.air_temp = (unsigned)para+40;
+}
+
+
+void iv::control::ControlStatus::set_air_mode(char para)
+{
+    command10.bit.air_mode = (unsigned)para;
+}
+
+void iv::control::ControlStatus::set_air_enable(bool enable)
+{
+    if(enable){
+        command10.bit.air_enable = 0x01;
+    }else{
+        command10.bit.air_enable = 0x00;
+    }
+}
+
+void iv::control::ControlStatus::set_wind_level(char para)
+{
+    command10.bit.air_wind_level = (unsigned)para;
+}
+
+void iv::control::ControlStatus::set_roof_light(char para)
+{
+    command10.bit.roof_light = (unsigned)para;
+}
+
+void iv::control::ControlStatus::set_home_light(char para)
+{
+    command10.bit.home_light = (unsigned)para;
+}
+
+void iv::control::ControlStatus::set_air_worktime(char para)
+{
+    command10.bit.air_work_time = (unsigned)para;
+}
+
+
+void iv::control::ControlStatus::set_air_offtime(char para)
+{
+    command10.bit.air_off_time = (unsigned)para;
+}
+
+
+//void iv::control::ControlStatus::set_speaker(bool enable){
+//    if (enable == true)
+//        command.bit.speaker = 1;
+//    else
+//        command.bit.speaker = 0;
+//}
+
+//void iv::control::ControlStatus::set_door(char enable){
+//    if (enable == true)
+//        command.bit.door = 1;
+//    else
+//        command.bit.door = 0;
+//}
+
+//void iv::control::ControlStatus::set_doorEnable(bool enable){
+//    if (enable == true){
+//        command.bit.doorEnable = 1;
+//        command.bit.door=1;
+//    }
+//    else{
+//        command.bit.doorEnable = 0;
+//        command.bit.door=0;
+//    }
+//}
+
+////void iv::control::ControlStatus::set_flicker(bool enable){
+////    if (enable == true)
+////        command.bit.flicker = 1;
+////    else
+////        command.bit.flicker = 0;
+////}
+//void iv::control::ControlStatus::set_light(bool enable){
+//    if (enable == true)
+//       command.bit.bright = 1;
+//    else
+//       command.bit.bright = 0;
+//}
+
+//void iv::control::ControlStatus::set_leftlight(bool enable){
+//    if (enable == true)
+//       command.bit.left_turn = 1;
+//    else
+//       command.bit.left_turn = 0;
+//}
+
+
+//void iv::control::ControlStatus::set_rightlight(bool enable){
+//    if (enable == true)
+//       command.bit.right_turn = 1;
+//    else
+//       command.bit.right_turn = 0;
+//}
+
+
+
+
+
+
+//void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
+//    if (left == true)
+//    {
+//        command.bit.left_turn = 1;
+//        command.bit.right_turn = 0;
+//    }
+//    else if (right == true)
+//    {
+//        command.bit.left_turn = 0;
+//        command.bit.right_turn = 1;
+//    }
+//    else
+//    {
+//        command.bit.left_turn = 0;
+//        command.bit.right_turn = 0;
+//    }
+//}
+
+
+
+
+
+
+
+
+//#else
+
+//void iv::control::ControlStatus::set_accelerate(float percent)
+//{
+//    command.bit.acce = (unsigned)((percent + 7) * 20);
+//}
+//void iv::control::ControlStatus::set_wheel_angle(float angle)
+//{
+//    command.bit.ang_H = angle * 10 >> 8;
+
+//    command.bit.ang_L = angle * 10 % 256;
+//}
+//void iv::control::ControlStatus::set_engine(char enable)
+//{
+//    command.bit.engine = enable;
+//}
+//void iv::control::ControlStatus::set_door(char enable){
+//    command.bit.door = enable;
+//}
+//void iv::control::ControlStatus::set_speaker(bool enable){
+//    if (enable == true)
+//        command.bit.speaker = 1;
+//    else
+//        command.bit.speaker = 0;
+//}
+//void iv::control::ControlStatus::set_flicker(bool enable){
+//    if (enable == true)
+//        command.bit.flicker = 1;
+//    else
+//        command.bit.flicker = 0;
+//}
+//void iv::control::ControlStatus::set_light(bool enable){
+//    if (enable == true)
+//       command.bit.bright = 1;
+//    else
+//       command.bit.bright = 0;
+//}
+//void iv::control::ControlStatus::set_turnsignals_control(bool left, bool right){
+//    if (left == true)
+//    {
+//        command.bit.left_turn = 1;
+//        command.bit.right_turn = 0;
+//    }
+//    else if (right == true)
+//    {
+//        command.bit.left_turn = 0;
+//        command.bit.right_turn = 1;
+//    }
+//    else
+//    {
+//        command.bit.left_turn = 0;
+//        command.bit.right_turn = 0;
+//    }
+//}
+//#endif

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/src/controller.cpp

@@ -0,0 +1,204 @@
+#include "adc_controller_shenlan_v2/controller.h"
+
+iv::control::Controller::Controller() {
+
+}
+
+iv::control::Controller::~Controller() {
+
+}
+
+void iv::control::Controller::inialize() {
+
+}
+
+
+
+
+
+//校验和
+void iv::control::Controller::cmd_checksum(unsigned char cmd_id) {
+    ServiceControlStatus.set_cmd_checksum(cmd_id);
+}
+//方向盘控制
+void iv::control::Controller::control_wheel(float angle) {
+    ServiceControlStatus.set_wheel_angle(angle);
+}
+
+void iv::control::Controller:: control_angle_speed(float angSpeed){
+    ServiceControlStatus.set_wheel_speed(angSpeed);
+}
+
+void iv::control::Controller:: control_angle_enable(bool enable){
+    ServiceControlStatus.set_wheel_enable(enable);
+}
+//速度限制
+void iv::control::Controller:: control_speed_limit(float speedLimit){
+    ServiceControlStatus.set_speed_limit(speedLimit);
+}
+//油门控制
+void iv::control::Controller::control_torque(float percent){
+    ServiceControlStatus.set_torque(percent);
+}
+void iv::control::Controller::control_acc_en(bool enanble){
+    ServiceControlStatus.set_acc_enable(enanble);
+}
+
+
+void iv::control::Controller::control_aeb(float aeb){
+    ServiceControlStatus.set_aeb(aeb);
+}
+void iv::control::Controller::control_aeb_en(bool enable){
+    ServiceControlStatus.set_aeb_enable(enable);
+}
+//刹车控制
+void iv::control::Controller::control_brake(float brake){
+    ServiceControlStatus.set_brake(brake);
+}
+//档位控制
+void iv::control::Controller::control_gear(float gear){
+    ServiceControlStatus.set_gear(gear);
+}
+void iv::control::Controller::control_gear_en(bool enable){
+    ServiceControlStatus.set_gear_enable(enable);
+}
+//手刹
+void iv::control::Controller::control_handBrake(bool  enable){
+    ServiceControlStatus.set_handBrake(enable);
+}
+//驾驶模式
+void iv::control::Controller::control_mode(char mode){
+    ServiceControlStatus.set_driveMode(mode);
+}
+//车窗控制
+//void iv::control::Controller::control_win_lf(char para){
+//     ServiceControlStatus.set_win_lf(para);
+//}
+//void iv::control::Controller::control_win_rf(char para){
+//     ServiceControlStatus.set_win_rf(para);
+//}
+
+//void iv::control::Controller::control_win_lr(char para){
+//     ServiceControlStatus.set_win_lr(para);
+//}
+
+//void iv::control::Controller::control_win_rr(char para){
+//     ServiceControlStatus.set_win_rr(para);
+//}
+
+//空调控制
+void iv::control::Controller::control_air_on(bool enable){
+    if(enable){
+        ServiceControlStatus.set_air_on(1);
+    }else{
+        ServiceControlStatus.set_air_on(0);
+    }
+}
+//void iv::control::Controller::control_air_cricle(char para){
+//     ServiceControlStatus.set_air_cricle(para);
+//}
+//void iv::control::Controller::control_air_auto(char para){
+//     ServiceControlStatus.set_air_auto(para);
+//}
+//void iv::control::Controller::control_air_off(char para){
+//     ServiceControlStatus.set_air_off(para);
+//}
+//void iv::control::Controller::control_air_temup(char para){
+//     ServiceControlStatus.set_air_temup(para);
+//}
+//void iv::control::Controller::control_air_temdown(char para){
+//     ServiceControlStatus.set_air_temdown(para);
+//}
+//void iv::control::Controller::control_air_powerup(char para){
+//     ServiceControlStatus.set_air_powerup(para);
+//}
+//void iv::control::Controller::control_air_powerdown(char para){
+//     ServiceControlStatus.set_air_powerdown(para);
+//}
+//点火控制
+void iv::control::Controller::control_obligate(char para){
+    ServiceControlStatus.set_obligate(para);
+}
+//车门控制
+void iv::control::Controller::control_door(char enable){
+    ServiceControlStatus.set_door(enable);
+}
+//车灯控制
+void iv::control::Controller::control_turnsignals(bool left, bool right){
+    ServiceControlStatus.set_turnsignals_control(left,right);
+}
+void iv::control::Controller::control_small_light(char para){
+    ServiceControlStatus.set_small_light(para);
+}
+void iv::control::Controller::control_near_light(char para){
+    ServiceControlStatus.set_near_light(para);
+}
+
+void iv::control::Controller::control_far_light(char para){
+    ServiceControlStatus.set_far_light(para);
+}
+void iv::control::Controller::control_frog_light(char para){
+    ServiceControlStatus.set_frog_light(para);
+}
+void iv::control::Controller::control_brake_light(char para){
+    ServiceControlStatus.set_brake_light(para);
+}
+void iv::control::Controller::control_defrog(char para){
+    ServiceControlStatus.set_defrog(para);
+}
+void iv::control::Controller::control_reverse_light(char para){
+    ServiceControlStatus.set_reverse_light(para);
+}
+
+//喇叭控制
+void iv::control::Controller::control_horn(char para){
+    ServiceControlStatus.set_horn(para);
+}
+//雨刷控制
+void iv::control::Controller::control_wiper(char para){
+    ServiceControlStatus.set_wiper(para);
+}
+
+
+void iv::control::Controller::control_air_temp(char para){
+    ServiceControlStatus.set_air_temp(para);
+}
+
+void iv::control::Controller::control_air_mode(char para){
+    ServiceControlStatus.set_air_mode(para);
+}
+
+void iv::control::Controller::control_air_enable(bool enable){
+    ServiceControlStatus.set_air_enable(enable);
+}
+
+void iv::control::Controller::control_wind_level(char para){
+    ServiceControlStatus.set_wind_level(para);
+}
+
+void iv::control::Controller::control_roof_light(char para){
+    ServiceControlStatus.set_roof_light(para);
+}
+
+void iv::control::Controller::control_home_light(char para){
+    ServiceControlStatus.set_home_light(para);
+}
+
+void iv::control::Controller::control_air_worktime(char para){
+    ServiceControlStatus.set_air_worktime(para);
+}
+
+void iv::control::Controller::control_air_offtime(char para){
+    ServiceControlStatus.set_air_offtime(para);
+}
+
+
+
+
+
+//void iv::control::Controller::control_flicker(bool enable){
+//    ServiceControlStatus.set_flicker(enable);
+//}
+//void iv::control::Controller::control_braking(float percent){
+
+//}

src/ros2/AutoWare2025/src/adc_gps_hcp2/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.8)
+project(adc_gps_hcp2)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+#find_package(adc_autoware_msgs REQUIRED)
+#find_package(autoware_auto_control_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
+find_package(Protobuf REQUIRED)
+find_package(Qt5 COMPONENTS Core SerialPort REQUIRED)
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  include
+  include/adc_gps_hcp2
+)
+
+#message(ERROR "catkin_INCLUDE_DIRS: ${catkin_INCLUDE_DIRS}")
+#message(STATUS "CMAKE_CURRENT_BINARY_DIR: ${CMAKE_CURRENT_BINARY_DIR}")
+
+add_executable(${PROJECT_NAME}
+  src/adc_gps_hcp2_node.cpp
+  src/adc_gps_hcp2_core.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}  Geographic)  
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs tf2_geometry_msgs
+  autoware_vehicle_msgs
+  Qt5Core Qt5SerialPort)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+  )
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ros2/AutoWare2025/src/adc_gps_hcp2/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/ros2/AutoWare2025/src/adc_gps_hcp2/include/adc_gps_hcp2/adc_gps_hcp2_core.hpp

@@ -0,0 +1,106 @@
+
+#ifndef ADC_GPS_HCP2_CORE_HPP_
+#define ADC_GPS_HCP2_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+//#include <QObject>
+#include <QThread>
+#include <QDateTime>
+#include <QTimer>
+#include <iostream>
+
+#include <QSerialPort>
+
+#include <thread>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
+
+using geometry_msgs::msg::AccelWithCovarianceStamped;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseStamped;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Twist;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using autoware_vehicle_msgs::msg::VelocityReport;
+
+class adc_gps_hcp2_core : public rclcpp::Node
+{
+public:
+    adc_gps_hcp2_core();
+    ~adc_gps_hcp2_core() = default;
+
+private:
+    void onTimer();
+
+
+private:
+    rclcpp::TimerBase::SharedPtr mptimer;
+
+
+private:
+    QByteArray mbaGPSBuf;
+    bool mbSerialOpen;
+    QSerialPort *m_serialPort_GPS;
+    int mnNotRecvCount;
+    qint64 mnLastOpenTime;
+
+    QString mstrHCP2;
+
+    double mfOldVel;
+    double mfCalc_acc;
+    int64_t mOldTime;
+
+    double mfacc;
+
+    std::thread * mpthread;
+    bool mbthreadrun = true;
+
+private:
+    void SerialGPSDecode();
+    void SerialGPSDecodeSen(QString strsen);
+
+    unsigned char calccheck(unsigned char * p);
+    void CalcXY(double flon,double flat,double & fx,double & fy);
+
+private:
+    std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+    std::string origin_frame_id_;     //!< @brief map frame_id
+    std::string mstrserialportname; 
+    bool mbUseVelocity = true;
+
+private:
+    rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
+    rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
+    rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
+//    rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+    rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+//    rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_twist_;
+
+private:
+    void publish_velocity(const VelocityReport & velocity);
+    void publish_odometry(const Odometry & odometry);
+    void publish_acceleration();
+    void publish_tf(const Odometry & odometry);
+
+    void ThreadRecvRTK();
+
+};
+
+#endif

src/ros2/AutoWare2025/src/adc_gps_hcp2/launch/adc_gps_hcp2.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="adc_gps_hcp2",
+            executable="adc_gps_hcp2",
+            name="adc_gps_hcp2",
+            output="screen",
+            emulate_tty=True,
+            parameters=[
+                {"SerialPortName": "/dev/ttyUSB0"},
+                {"UseVelocity": True}
+            ]
+        ),
+
+        Node(
+            package="adc_controller_shenlan_v2",
+            executable="adc_controller_shenlan_v2",
+            name="adc_controller_shenlan_v2",
+            output="screen",
+            emulate_tty=True
+        )
+    ])

src/ros2/AutoWare2025/src/adc_gps_hcp2/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>adc_gps_hcp2</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/AutoWare2025/src/adc_gps_hcp2/src/adc_gps_hcp2_core.cpp

@@ -0,0 +1,639 @@
+
+#include "adc_gps_hcp2_core.hpp"
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+using namespace std;
+
+static bool checknmeasen(const char * strsen,const unsigned int nlen)
+{
+    if(nlen< 4)return false;
+
+    int i;
+    char check;
+    int nstarpos = -1;
+    check = strsen[1]^strsen[2];
+    for(i=3;i<static_cast<int>(nlen);i++)
+    {
+        if(strsen[i] == '*')
+        {
+            nstarpos = i; 
+            break;
+        }
+        check = check^strsen[i];
+    }
+    if(nstarpos < 0)return false;
+    if(nstarpos >(static_cast<int>(nlen) -3))return false;
+    char strcheck[3];
+    int ncheck = check;
+    snprintf(strcheck,3,"%02X",ncheck);
+    char strsencheck[3];
+    strsencheck[2] = 0;
+    strsencheck[0] = strsen[nstarpos + 1];
+    strsencheck[1] = strsen[nstarpos + 2];
+    if(strncmp(strcheck,strsencheck,2) == 0)
+    {
+   //     qDebug("  r sen is %s",strsen);
+        return true;
+    }
+
+    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" cal check is "<<strcheck<<" sen check is "<<strsencheck<<std::endl;
+
+//    qDebug("  sen is %s",strsen);
+    return false;
+}
+
+struct Quaternion {
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
+
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
+
+
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+    // Abbreviations for the various angular functions
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    Quaternion q;
+    q.w = cy * cp * cr + sy * sp * sr;
+    q.x = cy * cp * sr - sy * sp * cr;
+    q.y = sy * cp * sr + cy * sp * cr;
+    q.z = sy * cp * cr - cy * sp * sr;
+
+    return q;
+}
+
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
+
+adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
+{
+ //   mTime.start();
+    mOldTime = std::chrono::system_clock::now().time_since_epoch().count();
+    mfCalc_acc = 0;
+    mfOldVel = 0;
+    mOldTime = 0;
+
+    mbSerialOpen = false;
+    mbaGPSBuf.clear();
+    mnNotRecvCount = 0;
+    mnLastOpenTime = 0;
+
+    simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
+    origin_frame_id_ = declare_parameter("origin_frame_id", "map");
+    mstrserialportname = declare_parameter("SerialPortName","/dev/ttyTHS1");
+    mbUseVelocity = declare_parameter("UseVelocity",true);
+
+    std::cout<<" use velocity: "<<mbUseVelocity<<std::endl;
+
+    static constexpr std::size_t queue_size = 1;
+    rclcpp::QoS durable_qos(queue_size);
+    durable_qos.transient_local();
+
+    pub_twist_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", durable_qos);
+    pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
+    pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
+    if(mbUseVelocity) pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
+    pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
+
+    std::cout<<" Serial Port Name: "<<mstrserialportname.data()<<std::endl;
+
+    QString struartdev = QString(mstrserialportname.data());
+    m_serialPort_GPS = new QSerialPort();
+    m_serialPort_GPS->setPortName(struartdev);
+    m_serialPort_GPS->setBaudRate(230400);
+    m_serialPort_GPS->setParity(QSerialPort::NoParity);
+    m_serialPort_GPS->setDataBits(QSerialPort::Data8);
+    m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
+    m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
+    m_serialPort_GPS->setReadBufferSize(0);
+
+    mpthread = new std::thread(&adc_gps_hcp2_core::ThreadRecvRTK,this);
+
+//    mptimer = create_wall_timer(1ms,std::bind(&adc_gps_hcp2_core::onTimer,this));
+}
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+//#include <asm/termios.h>
+#include <termios.h>
+
+static void set_baudrate (struct termios *opt, unsigned int baudrate)
+{
+	cfsetispeed(opt, baudrate);
+	cfsetospeed(opt, baudrate);
+}
+
+static void set_data_bit (struct termios *opt, unsigned int databit)
+{
+    opt->c_cflag &= ~CSIZE;
+    switch (databit) {
+    case 8:
+        opt->c_cflag |= CS8;
+        break;
+    case 7:
+        opt->c_cflag |= CS7;
+        break;
+    case 6:
+        opt->c_cflag |= CS6;
+        break;
+    case 5:
+        opt->c_cflag |= CS5;
+        break;
+    default:
+        opt->c_cflag |= CS8;
+        break;
+    }
+
+}
+
+static void set_parity (struct termios *opt, char parity)
+{
+    switch (parity) {
+    case 'N':                  /* no parity check */
+        opt->c_cflag &= ~PARENB;
+        break;
+    case 'E':                  /* even */
+        opt->c_cflag |= PARENB;
+        opt->c_cflag &= ~PARODD;
+        break;
+    case 'O':                  /* odd */
+        opt->c_cflag |= PARENB;
+        opt->c_cflag |= ~PARODD;
+        break;
+    default:                   /* no parity check */
+        opt->c_cflag &= ~PARENB;
+        break;
+    }
+}
+
+static void set_stopbit (struct termios *opt, const char *stopbit)
+{
+    if (0 == strcmp (stopbit, "1")) {
+        opt->c_cflag &= ~CSTOPB; /* 1 stop bit */
+    }	else if (0 == strcmp (stopbit, "1")) {
+        opt->c_cflag &= ~CSTOPB; /* 1.5 stop bit */
+    }   else if (0 == strcmp (stopbit, "2")) {
+        opt->c_cflag |= CSTOPB;  /* 2 stop bits */
+    } else {
+        opt->c_cflag &= ~CSTOPB; /* 1 stop bit */
+    }
+}
+
+//https://blog.csdn.net/morixinguan/article/details/80898172
+//串口设置
+int  set_port_attr (
+              int fd,
+              int  baudrate,          // B1200 B2400 B4800 B9600 .. B115200
+              int  databit,           // 5, 6, 7, 8
+              const char *stopbit,    //  "1", "1.5", "2"
+              char parity,            // N(o), O(dd), E(ven)
+              int vtime,
+              int vmin )
+{
+     struct termios opt;
+     tcgetattr(fd, &opt);
+     //设置波特率
+     set_baudrate(&opt, baudrate);
+     opt.c_cflag 		 |= CLOCAL | CREAD;      /* | CRTSCTS */
+     //设置数据位
+     set_data_bit(&opt, databit);
+     //设置校验位
+     set_parity(&opt, parity);
+     //设置停止位
+     set_stopbit(&opt, stopbit);
+     //其它设置
+     opt.c_oflag 		 = 0;
+     opt.c_lflag            	|= 0;
+     opt.c_oflag          	&= ~OPOST;
+     opt.c_cc[VTIME]     	 = vtime;
+     opt.c_cc[VMIN]         	 = vmin;
+     tcflush (fd, TCIFLUSH);
+     return (tcsetattr (fd, TCSANOW, &opt));
+}
+
+
+void adc_gps_hcp2_core::ThreadRecvRTK()
+{
+    std::cout<<" enter thread "<<std::endl;
+
+
+
+    int fd;
+	int len, i,ret;
+	(void)i;
+	(void)ret;
+ 
+ 
+	fd = open(mstrserialportname.data(), O_RDWR | O_NOCTTY);
+        if(fd < 0) {
+                perror(mstrserialportname.data());
+                return;
+        }
+
+    set_port_attr(fd,B230400,8,"1",'N',0,0);
+ 
+
+    char strbuf[1000];
+    bool bSerialPortOpen  = true;
+    (void)bSerialPortOpen;
+
+
+/*
+    bool bSerialPortOpen = false;
+    std::cout<<" open "<<std::endl;
+     if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
+    {
+        bSerialPortOpen = true;
+        std::cout<<" succ "<<std::endl;
+    }
+    else
+    {
+        std::cout<<" Open Port "<<gstrserialportname<<"  Fail."<<std::endl;
+        return;
+    }
+    */
+
+    while(mbthreadrun)
+    {
+  //      QByteArray ba = m_serialPort_GPS->readAll();
+
+        len = read(fd, strbuf, 999);
+        if(len > 0)
+        {
+            strbuf[len] = 0;
+     //       std::cout<<strbuf<<std::endl;
+     //       std::cout<<" ba len : "<<len<<std::endl;
+            mstrHCP2.append(strbuf);
+            SerialGPSDecode();
+         //   mnNotRecvCount = 0;
+            //Decode
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ //           std::cout<<" no data"<<std::endl;
+        }
+    }
+}
+
+void adc_gps_hcp2_core::onTimer()
+{
+    if(mbSerialOpen)
+    {
+        QByteArray ba = m_serialPort_GPS->readAll();
+
+        if(ba.length() > 0)
+        {
+            mstrHCP2.append(ba);
+            SerialGPSDecode();
+            mnNotRecvCount = 0;
+            //Decode
+        }
+        else
+        {
+            mnNotRecvCount++;
+            if(mnNotRecvCount > 1000)
+            {
+                m_serialPort_GPS->close();
+                mbSerialOpen = false;
+            }
+        }
+        return;
+    }
+
+    std::cout<<"run hear1 "<<std::endl;
+    if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
+    {
+        std::cout<<"run hear2."<<std::endl;
+        if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
+        {
+            mbSerialOpen = true;
+            std::cout<<" open port successfully."<<std::endl;
+        }
+        else
+        {
+            std::cout<<" open port fail."<<std::endl;
+        }
+        mnLastOpenTime = QDateTime::currentMSecsSinceEpoch();
+    }
+}
+
+
+
+unsigned char adc_gps_hcp2_core::calccheck(unsigned char *p)
+{
+    unsigned char rtn;
+    rtn = p[0]^p[1];
+    int i;
+    for(i=2;i<=56;i++)
+    {
+        rtn = rtn ^ p[i];
+    }
+    return rtn;
+}
+
+void adc_gps_hcp2_core::SerialGPSDecode()
+{
+    int xpos = mstrHCP2.indexOf('\n');
+    if(xpos>=0)
+    {
+
+        QString xsen = mstrHCP2.left(xpos+1);
+        SerialGPSDecodeSen(xsen);
+        mstrHCP2.remove(0,xpos+1);
+        SerialGPSDecode();
+    }
+}
+
+void adc_gps_hcp2_core::SerialGPSDecodeSen(QString strsen)
+{
+    QStringList strlistrmc;
+    strlistrmc = strsen.split(",");
+
+   if(strlistrmc.size() < 23)return;
+      if(strlistrmc.at(0) != "$GPCHC")return;
+
+   if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
+   {
+       int nPos = strsen.indexOf('$',10);
+       if(nPos > 0)
+       {
+           QString strnewsen = strsen.right(strsen.size()-nPos);
+//           qDebug("new sen is %s",strnewsen.toLatin1().data());
+           SerialGPSDecodeSen(strnewsen);
+       }
+       return;
+   }
+
+   double fheading,fLat,fLon,fVel,fPitch,fRoll;
+   double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
+   int nsv1,nsv2;
+   int gpsweek,gpstime;
+   int insstate,rtkstate;
+   (void)fHgt; (void)gyro_x; (void)gyro_y; (void)gyro_z; (void)acc_x; (void)acc_y; (void)acc_z;
+   (void)nsv1; (void)nsv2; (void)gpsweek; (void)gpstime; (void)insstate;
+   (void)fheading; (void)fLat; (void)fLon; (void)fPitch; (void)fRoll;
+   QString strx = strlistrmc.at(3);
+   fheading = strx.toDouble();
+
+   strx = strlistrmc.at(1);
+   gpsweek = strx.toInt();
+
+   strx = strlistrmc.at(2);
+   gpstime = strx.toInt();
+
+   strx = strlistrmc.at(12);
+   fLat = strx.toDouble();
+
+   strx  = strlistrmc.at(13);
+   fLon = strx.toDouble();
+
+   strx = strlistrmc.at(14);
+   fHgt = strx.toDouble();
+
+   double ve,vn,vu;
+   (void)ve; (void)vn; (void)vu;
+   strx = strlistrmc.at(15);
+   ve = strx.toDouble();
+
+   strx = strlistrmc.at(16);
+   vn = strx.toDouble();
+
+   strx = strlistrmc.at(17);
+   vu = strx.toDouble();
+
+//   fVel = sqrt(ve*ve + vn*vn + vu*vu);
+   strx = strlistrmc.at(18);
+   fVel = strx.toDouble();
+
+   int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+   int ntimediff = abs((nnow - mOldTime)/1e6);
+   if( ntimediff >= 100)
+ //  if((mTime.elapsed()-mOldTime) >= 100)
+   {
+       if(mOldTime == 0)
+       {
+           mfCalc_acc = 0;
+           mfOldVel = fVel;
+           mOldTime = nnow;
+       }
+       else
+       {
+           mfCalc_acc = (fVel - mfOldVel)/(ntimediff * 0.001);
+           mfOldVel = fVel;
+           mOldTime = nnow;
+       }
+   }
+
+   strx = strlistrmc.at(4);
+   fPitch = strx.toDouble();
+
+   strx = strlistrmc.at(5);
+   fRoll = strx.toDouble();
+
+   strx = strlistrmc.at(6);
+   gyro_x = strx.toDouble();
+
+   strx = strlistrmc.at(7);
+   gyro_y = strx.toDouble();
+
+   strx = strlistrmc.at(8);
+   gyro_z = strx.toDouble();
+
+   strx = strlistrmc.at(9);
+   acc_x = strx.toDouble();
+
+   strx = strlistrmc.at(10);
+   acc_y = strx.toDouble();
+
+   strx = strlistrmc.at(11);
+   acc_z = strx.toDouble();
+
+   strx = strlistrmc.at(19);
+   nsv1 = strx.toInt();
+
+   strx = strlistrmc.at(20);
+   nsv2 = strx.toInt();
+
+   strx = strlistrmc.at(21);
+   char strstate[3];
+   strstate[2] = 0;
+   if(strx.size() >= 2)
+   {
+
+       memcpy(strstate,strx.data(),2);
+  //     qDebug(strstate);
+       char xstate;
+       xstate = strstate[0];
+       rtkstate = 4;
+       int xs;
+       if(xstate == '0')xs = 0;
+       if(xstate == '1')xs = 3;
+       if(xstate == '2')xs = 4;
+       if(xstate == '3')xs = 8;
+       if(xstate == '4')xs = 6;
+       if(xstate == '5')xs = 5;
+       if(xstate == '6')xs = 3;
+       if(xstate == '7')xs = 4;
+       if(xstate == '8')xs = 5;
+       if(xstate == '9')xs = 5;
+
+       rtkstate = xs;
+//       if(mstate == 0)minsstate = 0;
+//       else minsstate = 4;
+
+       xstate = strstate[1];
+       if(xstate == '0')insstate = 3;
+       else insstate = 4;
+       if(rtkstate == 0)insstate = 3;
+   }
+
+    double flat = fLat;
+    double flon = fLon;
+    mfacc = acc_x;
+    double pitch,roll,yaw;
+    pitch = 0;
+    roll = 0;
+    yaw = (90 - fheading) *M_PI/180.0;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+    nav_msgs::msg::Odometry msg;
+    double fx,fy;
+    CalcXY(flon,flat,fx,fy);
+    double rel_x = fx;//7600;
+    double rel_y = fy;//32100;
+    double rel_z = 0;
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+    publish_odometry(msg);
+    publish_tf(msg);
+
+    autoware_vehicle_msgs::msg::VelocityReport velocity;
+    velocity.longitudinal_velocity = sqrt(pow(vn,2)+pow(ve,2));
+    velocity.lateral_velocity = 0.0F;
+    velocity.heading_rate = 0;
+    if(mbUseVelocity)publish_velocity(velocity);
+
+    std::cout<<" publish."<<std::endl;
+
+    publish_acceleration();
+
+
+}
+
+void adc_gps_hcp2_core::publish_velocity(const VelocityReport & velocity)
+{
+    VelocityReport msg = velocity;
+    msg.header.stamp = get_clock()->now();
+    msg.header.frame_id = simulated_frame_id_;
+    pub_velocity_->publish(msg);
+}
+void adc_gps_hcp2_core::publish_odometry(const Odometry & odometry)
+{
+    static int frame_id = 0;
+    frame_id++;
+    Odometry msg = odometry;
+    msg.header.frame_id = origin_frame_id_;
+    msg.header.stamp = get_clock()->now();
+    msg.child_frame_id = simulated_frame_id_;
+    pub_odom_->publish(msg);
+}
+
+void adc_gps_hcp2_core::publish_acceleration()
+{
+    AccelWithCovarianceStamped msg;
+    msg.header.frame_id = "/base_link";
+    msg.header.stamp = get_clock()->now();
+    msg.accel.accel.linear.x = mfacc;// 0;//vehicle_model_ptr_->getAx();
+
+    constexpr auto COV = 0.001;
+    msg.accel.covariance.at(0) = COV;          // linear x
+    msg.accel.covariance.at(1) = COV;          // linear y
+    msg.accel.covariance.at(2) = COV;          // linear z
+    msg.accel.covariance.at(21) = COV;    // angular x
+    msg.accel.covariance.at(28) = COV;  // angular y
+    msg.accel.covariance.at(35) = COV;      // angular z
+    pub_acc_->publish(msg);
+}
+
+void adc_gps_hcp2_core::publish_tf(const Odometry & odometry)
+{
+    static int frame_id = 0;
+    frame_id++;
+    TransformStamped tf;
+    tf.header.stamp = get_clock()->now();
+    tf.header.frame_id = origin_frame_id_;
+    tf.child_frame_id = simulated_frame_id_;
+    tf.transform.translation.x = odometry.pose.pose.position.x;
+    tf.transform.translation.y = odometry.pose.pose.position.y;
+    tf.transform.translation.z = odometry.pose.pose.position.z;
+    tf.transform.rotation = odometry.pose.pose.orientation;
+
+    tf2_msgs::msg::TFMessage tf_msg{};
+    tf_msg.transforms.emplace_back(std::move(tf));
+    pub_tf_->publish(tf_msg);
+}
+
+void adc_gps_hcp2_core::CalcXY(double flon,double flat,double & fx,double & fy)
+{
+    int zone{};
+    bool is_north{};
+    std::string mgrs_code;
+
+    double utm_x,utm_y;
+
+    try {
+        GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x, utm_y);
+        GeographicLib::MGRS::Forward(
+                    zone, is_north, utm_x, utm_y, flat, 3, mgrs_code);
+    } catch (const GeographicLib::GeographicErr & err) {
+        std::cerr << err.what() << std::endl;
+        return;
+
+    }
+    fx = fmod(utm_x,1e5);
+    fy = fmod(utm_y,1e5);
+    //  std::cout<<" fx: "<<fx<<" fy: "<<fy<<std::endl;
+    return;
+}
+

src/ros2/AutoWare2025/src/adc_gps_hcp2/src/adc_gps_hcp2_node.cpp

@@ -0,0 +1,17 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+//#include "adc_controller_shenlan_v2/adc_controller_shenlan_v2_core.hpp"
+#include "adc_gps_hcp2/adc_gps_hcp2_core.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<adc_gps_hcp2_core>());
+
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ros2/AutoWare2025/src/pilot_autoware_bridge/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.14)
+project(pilot_autoware_bridge)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+
+#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(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED) 
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+
+find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
+
+find_package(Protobuf REQUIRED)
+
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include/pilot_autoware_bridge
+  ${CMAKE_CURRENT_SOURCE_DIR}/../include/msgtype
+)
+link_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../lib
+)
+
+
+add_executable(pilot_autoware_bridge
+  src/pilot_autoware_bridge_node.cpp
+  src/pilot_autoware_bridge_core.cpp
+  src/simplesmshare.cpp
+  ./../include/msgtype/gpsimu.pb.cc
+  ./../include/msgtype/autowarectrlcmd.pb.cc
+  ./../include/msgtype/chassis.pb.cc
+  ./../include/msgtype/object.pb.cc
+  ./../include/msgtype/objectarray.pb.cc
+)
+
+target_link_libraries(pilot_autoware_bridge  ${Protobuf_LIBRARIES}  Geographic )  
+# modulecomm 
+
+ament_target_dependencies(pilot_autoware_bridge rclcpp std_msgs geometry_msgs 
+  tf2_geometry_msgs nav_msgs sensor_msgs autoware_control_msgs 
+   autoware_planning_msgs autoware_vehicle_msgs
+  autoware_utils  autoware_perception_msgs
+  tier4_perception_msgs)
+
+
+install(TARGETS
+pilot_autoware_bridge
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
+
+

src/ros2/AutoWare2025/src/pilot_autoware_bridge/README.md

@@ -0,0 +1,31 @@
+# pilot_autoware_bridge
+
+## Purpose
+
+This `pilot_autoware_bridge` is a bridge between pilot and autoware.
+
+
+
+## Inputs / Outputs
+
+### Input
+
+| Name | Type                            | Description                                       |
+| ---- | ------------------------------- | ------------------------------------------------- |
+| pose | geometry_msgs::msg::PoseStamped | pose source to used for the velocity calculation. |
+
+### Output
+
+| Name      | Type                                  | Description                                   |
+| --------- | ------------------------------------- | --------------------------------------------- |
+| twist     | geometry_msgs::msg::TwistStamped      | twist calculated from the input pose history. |
+| linear_x  | tier4_debug_msgs::msg::Float32Stamped | linear-x field of the output twist.           |
+| angular_z | tier4_debug_msgs::msg::Float32Stamped | angular-z field of the output twist.          |
+
+## Parameters
+
+none.
+
+## Assumptions / Known limits
+
+none.

src/ros2/AutoWare2025/src/pilot_autoware_bridge/include/pilot_autoware_bridge/pilot_autoware_bridge_core.hpp

@@ -0,0 +1,209 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PILOT_AUTOWARE_BRIDGE_CORE_HPP_
+#define PILOT_AUTOWARE_BRIDGE_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+
+#include "autoware_control_msgs/msg/control.hpp"
+//#include "autoware_auto_geometry_msgs/msg/complex32.hpp"
+//#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
+#include "autoware_vehicle_msgs/msg/engage.hpp"
+#include "autoware_vehicle_msgs/msg/gear_command.hpp"
+#include "autoware_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_vehicle_msgs/msg/hazard_lights_command.hpp"
+#include "autoware_vehicle_msgs/msg/hazard_lights_report.hpp"
+#include "autoware_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_vehicle_msgs/msg/turn_indicators_command.hpp"
+#include "autoware_vehicle_msgs/msg/turn_indicators_report.hpp"
+#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/ros/msg_covariance.hpp"
+#include "autoware_utils/ros/update_param.hpp"
+
+#include <autoware_control_msgs/msg/control.hpp>
+
+#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
+#include "gpsimu.pb.h"
+#include "autowarectrlcmd.pb.h"
+#include "chassis.pb.h"
+#include "objectarray.pb.h"
+
+//#include "modulecomm.h"
+
+#include <QSharedMemory>
+
+#include "simplesmshare.h"
+
+
+using autoware_control_msgs::msg::Control;
+//using autoware_auto_geometry_msgs::msg::Complex32;
+//using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_vehicle_msgs::msg::ControlModeReport;
+using autoware_vehicle_msgs::msg::Engage;
+using autoware_vehicle_msgs::msg::GearCommand;
+using autoware_vehicle_msgs::msg::GearReport;
+using autoware_vehicle_msgs::msg::HazardLightsCommand;
+using autoware_vehicle_msgs::msg::HazardLightsReport;
+using autoware_vehicle_msgs::msg::SteeringReport;
+using autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_vehicle_msgs::msg::TurnIndicatorsReport;
+using autoware_vehicle_msgs::msg::VelocityReport;
+
+using geometry_msgs::msg::AccelWithCovarianceStamped;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseStamped;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Twist;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+using autoware_control_msgs::msg::Control;
+
+class pilot_autoware_bridge : public rclcpp::Node
+{
+public:
+    pilot_autoware_bridge();
+    ~pilot_autoware_bridge() = default;
+
+private:
+    void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);
+    void callbackObject(tier4_perception_msgs::msg::DetectedObjectsWithFeature::SharedPtr pose_msg_ptr);
+    void callbackTimer();
+    void callbackTimerGPS();
+    void callbackTimerTestSim();
+
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+
+    rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
+    detected_object_with_feature_sub_;
+
+    rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
+    rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
+    rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;
+    rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
+    rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+    rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
+    rclcpp::Publisher<GearReport>::SharedPtr pub_gear_report_;
+    rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
+    rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
+    rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+    rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
+
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+    rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr detected_object_with_feature_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
+
+    rclcpp::Subscription<Control>::SharedPtr sub_cmd_;
+
+
+
+
+    rclcpp::TimerBase::SharedPtr mptimer;
+    rclcpp::TimerBase::SharedPtr mptimerGPS;
+    rclcpp::TimerBase::SharedPtr mptimerTestSim;
+
+    void publish_odometry(const Odometry & odometry);
+    void publish_tf(const Odometry & odometry);
+
+    /**
+   * @brief publish velocity
+   * @param [in] velocity The velocity report to publish
+   */
+    void publish_velocity(const VelocityReport & velocity);
+
+    /**
+   * @brief publish steering
+   * @param [in] steer The steering to publish
+   */
+    void publish_steering(const SteeringReport & steer);
+
+    void publish_acceleration();
+
+
+    void CalcXY(double flon,double flat,double & fx,double & fy);
+
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+    void ConvertObjAndPub(iv::lidar::objectarray * pobjarray);
+
+
+private:
+    std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+    std::string origin_frame_id_;     //!< @brief map frame_id
+
+    SteeringReport current_steer_;
+
+    Control::SharedPtr mpcmd_ptr;
+
+    bool mbcmdupdate = false;
+
+    void * mpagpsimu;
+    void * mpactrlcmd;
+    bool mbGPSAttach = false;
+    //  QSharedMemory * mpsharegps;
+
+    simplesmshare * mpsharegps;
+    simplesmshare * mpsharecmd;
+    simplesmshare * mpsharechassis;
+    simplesmshare * mpsharelidartrack;
+
+    bool mbTestSim = false;
+
+    double mfacc = 0;
+
+
+};
+
+#endif  

src/ros2/AutoWare2025/src/pilot_autoware_bridge/include/pilot_autoware_bridge/simplesmshare.h

@@ -0,0 +1,27 @@
+#ifndef SIMPLESMSHARE_H
+#define SIMPLESMSHARE_H
+
+#include <QSharedMemory>
+
+class simplesmshare
+{
+public:
+    simplesmshare(const char * strsmname,int nsize);
+
+    void create();
+    bool attach();
+
+    bool write(const char * strdata,int nsize);
+    int read(char * strdata,unsigned int nreadmax);
+
+private:
+    char mstrsmane[256];
+    int mnsize;
+
+    QSharedMemory * mpshare;
+
+    int64_t getcurrentms();
+
+};
+
+#endif // SIMPLESMSHARE_H

src/ros2/AutoWare2025/src/pilot_autoware_bridge/launch/pilot_autoware_bridge.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input_pose_topic" default="/localization/pose_estimator/pose" description=""/>
+  <arg name="output_twist_topic" default="/estimate_twist" description=""/>
+
+  <node pkg="testpose" exec="testpose" name="testpose" output="log">
+    <remap from="pose" to="$(var input_pose_topic)"/>
+    <remap from="twist" to="$(var output_twist_topic)"/>
+  </node>
+</launch>

src/ros2/AutoWare2025/src/pilot_autoware_bridge/package.xml

@@ -0,0 +1,33 @@
+<?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>pilot_autoware_bridge</name>
+  <version>0.1.0</version>
+  <description>testpose</description>
+  <maintainer email="yuchuli@catarc.ac.cn">Yu Chuli</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="yuchuli@catarc.ac.cn">Yu Chuli</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_mapping_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_tf2</depend>
+  <depend>autoware_vehicle_msgs</depend>
+
+    <depend>tier4_api_utils</depend>
+  <depend>autoware_utils</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
+  <depend>tier4_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rclcpp</depend>
+
+
+ <export>
+   <build_type>ament_cmake</build_type>
+ </export>
+</package>

src/ros2/AutoWare2025/src/pilot_autoware_bridge/src/pilot_autoware_bridge_core.cpp

@@ -0,0 +1,866 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "pilot_autoware_bridge/pilot_autoware_bridge_core.hpp"
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <cmath>
+#include <cstddef>
+#include <functional>
+
+#include <time.h>
+#include <sstream>
+
+#include <iostream>
+
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
+#include <QString>
+
+#include <chrono>
+
+//#include "modulecomm.h"   
+using namespace std;
+
+static double gfchassis_acc = 0;
+
+
+pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
+{
+    QString str;
+    using std::placeholders::_1;
+
+    simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
+    origin_frame_id_ = declare_parameter("origin_frame_id", "map");
+
+    static constexpr std::size_t queue_size = 1;
+    rclcpp::QoS durable_qos(queue_size);
+    durable_qos.transient_local();
+
+    twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", durable_qos);
+    // Note: this callback publishes topics above
+    pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
+                "pose", queue_size, std::bind(&pilot_autoware_bridge::callbackPose, this, _1));
+
+//    detected_object_with_feature_sub_ = create_subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(
+//                "/perception/object_recognition/detection/labeled_clusters", queue_size, std::bind(&pilot_autoware_bridge::callbackObject, this, _1));
+
+
+    pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/perception/obstacle_segmentation/pointcloud", durable_qos);
+
+    
+ //   detected_object_with_feature_pub_ = create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>("/perception/object_recognition/detection/labeled_clusters", durable_qos);
+    detected_object_with_feature_pub_ = create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>("/perception/object_recognition/detection/objects_with_feature", durable_qos);
+    pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
+    pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
+
+    pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
+
+    pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", durable_qos);
+    pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
+
+    sub_cmd_ = create_subscription<Control>(
+                "/control/command/control_cmd", 1, [this](const Control::SharedPtr msg) { mpcmd_ptr = msg;mbcmdupdate = true; });
+
+
+    mpsharecmd =  new simplesmshare("aw_ctrlcmd",1000);
+    mpsharecmd->create();
+    mpsharecmd->attach();
+
+    mpsharechassis = new simplesmshare("aw_chassis",1000);
+    mpsharegps = new simplesmshare("aw_gps",10000);
+    mpsharelidartrack = new simplesmshare("aw_lidartrack",20000000);
+
+
+    mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
+    mptimerGPS = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerGPS, this));
+
+    if(mbTestSim)
+    {
+        mptimerTestSim = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerTestSim,this));
+    }
+    // ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    // mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+
+    // mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
+
+
+
+
+}
+
+double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
+{
+    double diff_rad = lhs_rad - rhs_rad;
+    if (diff_rad > M_PI) {
+        diff_rad = diff_rad - 2 * M_PI;
+    } else if (diff_rad < -M_PI) {
+        diff_rad = diff_rad + 2 * M_PI;
+    }
+    return diff_rad;
+}
+
+// x: roll, y: pitch, z: yaw
+geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose)
+{
+    geometry_msgs::msg::Vector3 rpy;
+    tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+    tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+    return rpy;
+}
+
+geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose)
+{
+    return getRPY(pose->pose);
+}
+
+geometry_msgs::msg::TwistStamped calcTwist(
+        geometry_msgs::msg::PoseStamped::SharedPtr pose_a,
+        geometry_msgs::msg::PoseStamped::SharedPtr pose_b)
+{
+    const double dt =
+            (rclcpp::Time(pose_b->header.stamp) - rclcpp::Time(pose_a->header.stamp)).seconds();
+
+    if (dt == 0) {
+        geometry_msgs::msg::TwistStamped twist;
+        twist.header = pose_b->header;
+        return twist;
+    }
+
+    const auto pose_a_rpy = getRPY(pose_a);
+    const auto pose_b_rpy = getRPY(pose_b);
+
+    geometry_msgs::msg::Vector3 diff_xyz;
+    geometry_msgs::msg::Vector3 diff_rpy;
+
+    diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
+    diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
+    diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
+    diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
+    diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
+    diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);
+
+    geometry_msgs::msg::TwistStamped twist;
+    twist.header = pose_b->header;
+    twist.twist.linear.x =
+            std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) /
+            dt;
+    twist.twist.linear.y = 0;
+    twist.twist.linear.z = 0;
+    twist.twist.angular.x = diff_rpy.x / dt;
+    twist.twist.angular.y = diff_rpy.y / dt;
+    twist.twist.angular.z = diff_rpy.z / dt;
+
+    return twist;
+}
+
+struct Quaternion {
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
+
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
+
+double steear_now = 0;
+
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+    // Abbreviations for the various angular functions
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    Quaternion q;
+    q.w = cy * cp * cr + sy * sp * sr;
+    q.x = cy * cp * sr - sy * sp * cr;
+    q.y = sy * cp * sr + cy * sp * cr;
+    q.z = sy * cp * cr - cy * sp * sr;
+
+    return q;
+}
+
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
+
+void pilot_autoware_bridge::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
+{
+    (void)pose_msg_ptr;
+    // TODO(YamatoAndo) check time stamp diff
+    // TODO(YamatoAndo) check suddenly move
+    // TODO(YamatoAndo) apply low pass filter
+
+
+}
+
+
+void pilot_autoware_bridge::callbackTimerGPS()
+{
+
+
+    unsigned int nSize;
+    char * strdata = new char[20000000];
+
+    int nread = mpsharegps->read(strdata,10000);
+
+    int nogpscount = 0;
+
+    if(nread < 1)
+    {
+        nogpscount++;
+        if(nogpscount >= 100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" no gps."<<std::endl;
+            nogpscount = 0;
+        }
+    }
+    else
+    {
+
+        nSize = nread;
+
+ //       std::cout<<" nSize: "<<nSize<<std::endl;
+
+        iv::gps::gpsimu  xgpsimu;
+        if(!xgpsimu.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<" parse gps msg fail."<<std::endl;
+            return;
+        }
+        else
+        {
+
+            double flat = xgpsimu.lat();
+            double flon = xgpsimu.lon();
+
+            mfacc = gfchassis_acc;//xgpsimu.acce_x();
+
+            double pitch,roll,yaw;
+            pitch = 0;
+            roll = 0;
+
+            yaw = (90 - xgpsimu.heading()) *M_PI/180.0;
+            Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+            nav_msgs::msg::Odometry msg;
+
+            double fx,fy;
+            CalcXY(flon,flat,fx,fy);
+
+            double rel_x = fx;//7600;
+            double rel_y = fy;//32100;
+            double rel_z = 0;
+            double vn = xgpsimu.vn();
+            double ve = xgpsimu.ve();
+            // rel_x+=0.01;
+
+            msg.pose.pose.position.x = rel_x;
+            msg.pose.pose.position.y = rel_y;
+            msg.pose.pose.position.z = rel_z;
+
+            msg.pose.pose.orientation.x = quat.x;
+            msg.pose.pose.orientation.y = quat.y;
+            msg.pose.pose.orientation.z = quat.z;
+            msg.pose.pose.orientation.w = quat.w;
+
+            msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+            publish_odometry(msg);
+            publish_tf(msg);
+
+            autoware_vehicle_msgs::msg::VelocityReport velocity;
+            velocity.longitudinal_velocity = sqrt(pow(vn,2)+pow(ve,2));
+            velocity.lateral_velocity = 0.0F;
+            velocity.heading_rate = 0;
+            publish_velocity(velocity);
+
+            current_steer_.steering_tire_angle = static_cast<double>(steear_now);
+            current_steer_.stamp = get_clock()->now();
+
+            publish_steering(current_steer_);
+
+            publish_acceleration();
+
+
+ //           std::cout<<" share gps."<<std::endl;
+            nogpscount = 0;
+        }
+
+    }
+
+    int nochassiscount = 0;
+
+    nread = mpsharechassis->read(strdata,10000);
+    
+
+    if(nread < 1)
+    {
+        nochassiscount++;
+        if(nochassiscount >= 100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" no chassis."<<std::endl;
+            nochassiscount = 0;
+        }
+    }
+    else
+    {
+        static int64_t nchassislast = 0;
+        static double fchassis_veh_last = 0;
+        iv::chassis xchassis;
+        //    static int ncount = 0;
+        if(!xchassis.ParseFromArray(strdata,nread))
+        {
+            std::cout<<"Chassis ParseFrom Array Error."<<std::endl;
+        }
+        else
+        {
+            steear_now = (xchassis.angle_feedback() *M_PI/180.0)/15.0;
+            double fchassis_veh_now = xchassis.vel()/3.6;
+            int64_t nchassis_now = xchassis.time();
+            int64_t ndiff = nchassis_now - nchassislast;
+            double fdiff = ndiff/1e9;
+            if(fdiff >= 0.1)
+            {
+                gfchassis_acc = (fchassis_veh_now - fchassis_veh_last) /fdiff;
+                std::cout<<" calc acc: "<<gfchassis_acc<<std::endl;
+                nchassislast = nchassis_now;
+                fchassis_veh_last = fchassis_veh_now;
+  //              std::cout<<" acc: "<<gfchassis_acc<<std::endl;
+            }
+            else
+            {
+      //          std::cout<<" fdiff: "<<fdiff<<"ndiff: "<<ndiff<<std::endl;
+    //            gfchassis_acc = 0;
+            }
+
+            nochassiscount = 0;
+  //          if(steear_now < 0.13)std::cout<<nchassis_now<<" steer : "<<steear_now<<std::endl;
+        }
+
+    }
+
+    int noobjectcount = 0;
+
+    nread = mpsharelidartrack->read(strdata,20000000);
+    if(nread<1)
+    {
+        noobjectcount++;
+        if(noobjectcount >= 100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" no object."<<std::endl;
+        }
+    }
+    else
+    {
+        iv::lidar::objectarray xobjarray;
+        if(!xobjarray.ParseFromArray(strdata,nread))
+        {
+            std::cout<<"Lidar Object Parse From Array Fail."<<std::endl;
+        }
+        else
+        {
+
+            ConvertObjAndPub(&xobjarray);
+            noobjectcount = 0;
+        }
+    }
+
+    delete strdata;
+
+
+
+
+    
+}
+
+void pilot_autoware_bridge::callbackTimer()
+{
+   // std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" testpose1. "<<std::endl;
+
+
+    if(mbcmdupdate == true)
+    {
+        const auto steer = mpcmd_ptr->lateral.steering_tire_angle;
+        const auto vel = mpcmd_ptr->longitudinal.velocity;
+        const auto accel = mpcmd_ptr->longitudinal.acceleration;
+        iv::autoware::autowarectrlcmd xcmd;
+        xcmd.set_linear_acceleration(accel);
+        xcmd.set_linear_velocity(vel);
+        xcmd.set_steering_angle(steer);
+        // steear_now = steer;
+        int ndatasize = xcmd.ByteSizeLong();
+        std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[ndatasize]);
+        if(xcmd.SerializeToArray(pstr_ptr.get(),ndatasize))
+        {
+            mpsharecmd->write(pstr_ptr.get(),ndatasize);
+        }
+        else
+        {
+            std::cout<<" command serialize fail."<<std::endl;
+        }
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" stear: "<<steer<<" vel: "<<vel<<" accel: "<<accel<<std::endl;
+        mbcmdupdate = false;
+    }
+    return;
+
+
+}
+
+void pilot_autoware_bridge::publish_odometry(const Odometry & odometry)
+{
+    static int frame_id = 0;
+    frame_id++;
+    Odometry msg = odometry;
+    msg.header.frame_id = origin_frame_id_;
+    msg.header.stamp = get_clock()->now();
+    msg.child_frame_id = simulated_frame_id_;
+    pub_odom_->publish(msg);
+
+
+}
+
+void pilot_autoware_bridge::publish_tf(const Odometry & odometry)
+{
+    static int frame_id = 0;
+    frame_id++;
+    TransformStamped tf;
+    tf.header.stamp = get_clock()->now();
+    tf.header.frame_id = origin_frame_id_;
+    tf.child_frame_id = simulated_frame_id_;
+    tf.transform.translation.x = odometry.pose.pose.position.x;
+    tf.transform.translation.y = odometry.pose.pose.position.y;
+    tf.transform.translation.z = odometry.pose.pose.position.z;
+    tf.transform.rotation = odometry.pose.pose.orientation;
+
+    tf2_msgs::msg::TFMessage tf_msg{};
+    tf_msg.transforms.emplace_back(std::move(tf));
+    pub_tf_->publish(tf_msg);
+}
+
+void pilot_autoware_bridge::publish_velocity(const VelocityReport & velocity)
+{
+    VelocityReport msg = velocity;
+    msg.header.stamp = get_clock()->now();
+    msg.header.frame_id = simulated_frame_id_;
+    pub_velocity_->publish(msg);
+}
+
+void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double & fy)
+{
+    int zone{};
+    bool is_north{};
+    std::string mgrs_code;
+
+    double utm_x,utm_y;
+
+    try {
+        GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x, utm_y);
+        GeographicLib::MGRS::Forward(
+                    zone, is_north, utm_x, utm_y, flat, 3, mgrs_code);
+    } catch (const GeographicLib::GeographicErr & err) {
+        std::cerr << err.what() << std::endl;
+        return;
+
+    }
+    fx = fmod(utm_x,1e5);
+    fy = fmod(utm_y,1e5);
+    //  std::cout<<" fx: "<<fx<<" fy: "<<fy<<std::endl;
+    return;
+}
+
+
+
+
+void pilot_autoware_bridge::publish_steering(const SteeringReport & steer)
+{
+
+    pub_steer_->publish(steer);
+}
+
+void pilot_autoware_bridge::publish_acceleration()
+{
+    AccelWithCovarianceStamped msg;
+    msg.header.frame_id = "/base_link";
+    msg.header.stamp = get_clock()->now();
+    msg.accel.accel.linear.x = mfacc;// 0;//vehicle_model_ptr_->getAx();
+
+    using COV_IDX = autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+    constexpr auto COV = 0.001;
+    msg.accel.covariance.at(COV_IDX::X_X) = COV;          // linear x
+    msg.accel.covariance.at(COV_IDX::Y_Y) = COV;          // linear y
+    msg.accel.covariance.at(COV_IDX::Z_Z) = COV;          // linear z
+    msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV;    // angular x
+    msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV;  // angular y
+    msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV;      // angular z
+    pub_acc_->publish(msg);
+}
+
+void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    std::cout<<" recv gps."<<std::endl;
+
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        
+        return;
+    }
+
+    double flat = xgpsimu.lat();
+    double flon = xgpsimu.lon();
+
+    mfacc = gfchassis_acc; //xgpsimu.acce_x();
+
+    double pitch,roll,yaw;
+    pitch = 0;
+    roll = 0;
+
+    yaw = 0;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+    nav_msgs::msg::Odometry msg;
+
+    double fx,fy;
+    CalcXY(flon,flat,fx,fy);
+
+    static double rel_x = fx;//7600;
+    static double rel_y = fy;//32100;
+    static double rel_z = 0;
+    static double vn = 0;
+    static double ve = 1;
+    // rel_x+=0.01;
+
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+    publish_odometry(msg);
+    publish_tf(msg);
+
+    autoware_vehicle_msgs::msg::VelocityReport velocity;
+    velocity.longitudinal_velocity = 0;
+    velocity.lateral_velocity = 0.0F;
+    velocity.heading_rate = 0;
+    publish_velocity(velocity);
+
+    current_steer_.steering_tire_angle = static_cast<double>(0);
+    current_steer_.stamp = get_clock()->now();
+
+    publish_steering(current_steer_);
+
+    publish_acceleration();
+}
+
+
+void pilot_autoware_bridge::callbackObject(tier4_perception_msgs::msg::DetectedObjectsWithFeature::SharedPtr obj_msg_ptr)
+{
+    (void)obj_msg_ptr;
+    std::cout<<"recv obj. "<<std::endl;
+    std::cout<<"obj cout: "<<obj_msg_ptr->feature_objects.size()<<std::endl;
+    tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+    int nsize = obj_msg_ptr->feature_objects.size();
+    int i;
+    for(i=0;i<nsize;i++)
+    {
+        feature_object = obj_msg_ptr->feature_objects.at(i);
+
+        std::cout<<" class:  "<<feature_object.object.classification.size()<<std::endl;
+        if(feature_object.object.classification.size()>0)
+        {
+            std::cout<<" label: "<<(int)feature_object.object.classification.at(0).label<<"probability"<<feature_object.object.classification.at(0).probability<<std::endl;
+        }
+
+        geometry_msgs::msg::PoseWithCovariance pose;
+        geometry_msgs::msg::TwistWithCovariance twist;
+
+        pose = feature_object.object.kinematics.pose_with_covariance;
+        twist = feature_object.object.kinematics.twist_with_covariance;
+        std::cout<<" x:"<< pose.pose.position.x<<" y: "<<pose.pose.position.y<<std::endl;;
+        std::cout<<" vel: "<<twist.twist.linear.x<<std::endl;
+
+        autoware_perception_msgs::msg::Shape shape= feature_object.object.shape;
+        std::cout<<"dim: x:"<<shape.dimensions.x<<" y:"<<shape.dimensions.y<<" z:"<<shape.dimensions.z<<std::endl;
+        std::cout<<" shape type: "<<(int)shape.type<<std::endl;
+        int j;
+        int sizepoint = shape.footprint.points.size();
+        std::cout<<" shape point size: "<<sizepoint<<std::endl;
+        for(j=0;j<sizepoint;j++)
+        {
+            std::cout<<" index"<<j<<" : x: "<<shape.footprint.points.at(j).x<<" y: "<<shape.footprint.points.at(j).y<<std::endl;
+        }
+
+        //        feature_object.object.classification.push_back(object.classification);
+        //        feature_object.object.kinematics.pose_with_covariance = object.initial_state.pose_covariance;
+        //        feature_object.object.kinematics.twist_with_covariance =
+        //          object.initial_state.twist_covariance;
+        //        feature_object.object.kinematics.orientation_availability =
+        //          autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+        //        feature_object.object.kinematics.has_twist = false;
+        //        tf2::toMsg(
+        //          tf_base_link2noised_moved_object,
+        //          feature_object.object.kinematics.pose_with_covariance.pose);
+        //        feature_object.object.shape = object.shape;
+    }
+}
+
+void pilot_autoware_bridge::callbackTimerTestSim()
+{
+    double pitch,roll,yaw;
+    pitch = 0;
+    roll = 0;
+
+    yaw = 0;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+    nav_msgs::msg::Odometry msg;
+
+    double flon,flat;
+    flon = 117.0857970;
+    flat = 39.1366668;
+
+    double fx,fy;
+    CalcXY(flon,flat,fx,fy);
+
+    static double rel_x = fx;//7600;
+    static double rel_y = fy;//32100;
+    static double rel_z = 0;
+    static double vn =  0;
+    static double ve =  1;
+    // rel_x+=0.01;
+
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+    publish_odometry(msg);
+    publish_tf(msg);
+
+    autoware_vehicle_msgs::msg::VelocityReport velocity;
+    velocity.longitudinal_velocity = 0;
+    velocity.lateral_velocity = 0.0F;
+    velocity.heading_rate = 0;
+    publish_velocity(velocity);
+
+    current_steer_.steering_tire_angle = static_cast<double>(steear_now);
+    current_steer_.stamp = get_clock()->now();
+
+    publish_steering(current_steer_);
+
+    publish_acceleration();
+
+    geometry_msgs::msg::PoseWithCovariance pose;
+    geometry_msgs::msg::TwistWithCovariance twist;
+
+    pose.pose.position.set__x(10);
+    pose.pose.position.set__y(0);
+    pose.pose.position.set__z(0);
+
+    pose.pose.orientation.set__w(quat.w);
+    pose.pose.orientation.set__x(quat.x);
+    pose.pose.orientation.set__y(quat.y);
+    pose.pose.orientation.set__z(quat.z);
+
+    twist.twist.linear.set__x(0);
+
+
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg;
+
+    tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+    autoware_perception_msgs::msg::ObjectClassification xclass;
+    xclass.set__label(autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
+    xclass.set__probability(1.0);
+    feature_object.object.classification.push_back(xclass);
+    feature_object.object.kinematics.pose_with_covariance = pose;
+    feature_object.object.kinematics.twist_with_covariance = twist;
+    feature_object.object.kinematics.orientation_availability =
+      autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+    feature_object.object.kinematics.has_twist = false;
+
+
+
+ //   feature_object.object.shape = object.shape;
+
+
+    rclcpp::Time current_time = this->now();
+    output_dynamic_object_msg.feature_objects.push_back(feature_object);
+    output_dynamic_object_msg.header.frame_id = "base_link";
+    output_dynamic_object_msg.header.stamp = current_time;
+    detected_object_with_feature_pub_->publish(output_dynamic_object_msg);
+
+
+    sensor_msgs::msg::PointCloud2 output_pointcloud_msg;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg);
+
+    output_pointcloud_msg.header.frame_id = "base_link";
+    output_pointcloud_msg.header.stamp = current_time;
+    // publish
+    pointcloud_pub_->publish(output_pointcloud_msg);
+
+
+
+}
+
+void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
+{
+
+    sensor_msgs::msg::PointCloud2 output_pointcloud_msg;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg;
+
+    int nobjsize = pobjarray->obj_size();
+
+    int i;
+    for(i=0;i<nobjsize;i++)
+    {
+        iv::lidar::lidarobject * pobj = pobjarray->mutable_obj(i);
+        geometry_msgs::msg::PoseWithCovariance pose;
+        geometry_msgs::msg::TwistWithCovariance twist;
+
+        pose.pose.position.set__x(pobj->position().x());
+        pose.pose.position.set__y(pobj->position().y());
+        pose.pose.position.set__x(pobj->position().y());
+        pose.pose.position.set__y(pobj->position().x() *(-1.0));
+        pose.pose.position.set__z(pobj->position().z() * 0.0);
+
+        double pitch,roll,yaw;
+        pitch = 0;
+        roll = 0;
+        yaw = pobj->tyaw() - M_PI/2.0;
+        Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+        pose.pose.orientation.set__w(quat.w);
+        pose.pose.orientation.set__x(quat.x);
+        pose.pose.orientation.set__y(quat.y);
+        pose.pose.orientation.set__z(quat.z);
+
+        twist.twist.linear.set__x(0);
+
+/*
+        int ncloudsize = pobj->cloud_size();
+        int j;
+        for(j=0;j<ncloudsize;j++)
+        {
+            pcl::PointXYZ point;
+            point.x = pobj->cloud(j).x();
+            point.y = pobj->cloud(j).y();
+            point.z = pobj->cloud(j).z();
+            merged_pointcloud_ptr->points.push_back(point);
+            ++merged_pointcloud_ptr->width;
+        }
+*/
+
+        tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+        autoware_perception_msgs::msg::ObjectClassification xclass;
+        xclass.set__label(autoware_perception_msgs::msg::ObjectClassification::UNKNOWN);
+        if(pobj->type_name() == "pedestrian")
+            xclass.set__label(autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
+        if(pobj->type_name() == "car")
+            xclass.set__label(autoware_perception_msgs::msg::ObjectClassification::CAR);
+        if(pobj->type_name() == "bike")
+            xclass.set__label(autoware_perception_msgs::msg::ObjectClassification::BICYCLE);
+        xclass.set__probability(1.0);
+        feature_object.object.classification.push_back(xclass);
+        feature_object.object.kinematics.pose_with_covariance = pose;
+        feature_object.object.kinematics.twist_with_covariance = twist;
+        feature_object.object.kinematics.orientation_availability =
+          autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+        feature_object.object.kinematics.has_twist = false;
+     //   feature_object.object.shape = object.shape;
+
+        // shape
+
+        autoware_perception_msgs::msg::Shape shape;
+        shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
+        shape.dimensions.x = pobj->dimensions().x();
+        shape.dimensions.y = pobj->dimensions().y();
+        shape.dimensions.z = pobj->dimensions().z();
+
+        feature_object.object.set__shape(shape);
+
+ //       std::cout<<" set shape. "<<std::endl;
+
+        output_dynamic_object_msg.feature_objects.push_back(feature_object);
+
+    }
+
+
+
+
+
+    rclcpp::Time current_time = this->now();
+
+    output_dynamic_object_msg.header.frame_id = "base_link";
+    output_dynamic_object_msg.header.stamp = current_time;
+    detected_object_with_feature_pub_->publish(output_dynamic_object_msg);
+
+
+
+    pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg);
+
+    output_pointcloud_msg.header.frame_id = "base_link";
+    output_pointcloud_msg.header.stamp = current_time;
+    // publish
+  //  pointcloud_pub_->publish(output_pointcloud_msg);
+}
+

src/ros2/AutoWare2025/src/pilot_autoware_bridge/src/pilot_autoware_bridge_node.cpp

@@ -0,0 +1,29 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "pilot_autoware_bridge/pilot_autoware_bridge_core.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<pilot_autoware_bridge>());
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ros2/AutoWare2025/src/pilot_autoware_bridge/src/simplesmshare.cpp

@@ -0,0 +1,130 @@
+#include "pilot_autoware_bridge/simplesmshare.h"
+
+#include <thread>
+#include <iostream>
+#include <chrono>
+
+simplesmshare::simplesmshare(const char * strsmname,int nsize)
+{
+    strncpy(mstrsmane,strsmname,255);
+    mnsize = nsize;
+    mpshare = new QSharedMemory(mstrsmane);
+}
+
+
+void simplesmshare::create()
+{
+    mpshare->create(mnsize);
+}
+
+bool simplesmshare::attach()
+{
+    return mpshare->attach();
+}
+
+bool simplesmshare::write(const char * strdata,int nsize)
+{
+
+    if(!mpshare->isAttached())
+    {
+        mpshare->create(mnsize);
+        mpshare->attach();
+    }
+
+    if(!mpshare->isAttached())
+    {
+        std::cout<<getcurrentms()<<" simplesmshare::write not attach"<<std::endl;
+        return false;
+    }
+
+    if(nsize >  (mnsize - 16))
+    {
+        std::cout<<" data size is big. buffer size is: "<<(mnsize-16)<<" data size is: "<<nsize<<std::endl;
+        return false;
+    }
+
+    int npostime = sizeof(int);
+    int npossize = sizeof(int64_t) + npostime;
+    int nposdata = npossize + sizeof(int);
+
+    char * pmem = (char *)mpshare->data();
+    int * pmark = (int * )pmem;
+    int64_t * ptime = (int64_t * )(pmem + npostime);
+    int * psize = (int * )(pmem + npossize);
+    char * pdata = (char * )(pmem + nposdata);
+    *pmark = 1;
+    *ptime = std::chrono::system_clock::now().time_since_epoch().count();
+    *psize = nsize;
+    memcpy(pdata,strdata,nsize);
+    *pmark = 0;
+
+ //   std::cout<<"write msg. "<<std::endl;
+
+    return true;
+}
+
+int simplesmshare::read(char * strdata,unsigned int nreadmax)
+{
+    if(!mpshare->isAttached())
+    {
+        mpshare->attach();
+    }
+    if(!mpshare->isAttached())
+    {
+        std::cout<<getcurrentms()<<" simplesmshare::read not attach"<<std::endl;
+        return 0;
+    }
+
+    static int64_t nLastUpdate = 0;
+
+    int npostime = sizeof(int);
+     int npossize = sizeof(int64_t) + npostime;
+     int nposdata = npossize + sizeof(int);
+
+     char * pmem = (char *)mpshare->data();
+     int * pmark = (int * )pmem;
+     int64_t * ptime = (int64_t * )(pmem + npostime);
+     int * psize = (int * )(pmem + npossize);
+     char * pdata = (char * )(pmem + nposdata);
+
+     if(*pmark == 1)return 0;
+
+
+
+     if(*ptime == nLastUpdate)
+     {
+         return 0;
+     }
+
+     int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+     if(abs(*ptime -nnow)/1000000 > 30000)
+     {
+         std::cout<<" data is more than 30 seconds."<<std::endl;
+         return 0;
+     }
+
+     nLastUpdate = *ptime;
+
+
+
+
+
+     unsigned int nSize;
+     nSize = *psize ;
+
+     if(nSize > nreadmax)
+     {
+         std::cout<<" data size is: "<<nSize<<" nread is : "<<nreadmax<<". please change nread."<<std::endl;
+         return 0;
+     }
+
+ //    std::cout<<" nSize: "<<nSize<<std::endl;
+     memcpy(strdata,pdata,nSize);
+
+     return nSize;
+}
+
+int64_t simplesmshare::getcurrentms()
+{
+    return std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+}