rtk_asensing

src/perception/rtk_asensing/CMakeLists.txt

@@ -21,17 +21,18 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sweeper_interfaces REQUIRED)
 find_package(logger REQUIRED)
+find_package(imu_msgs REQUIRED)
 
 include_directories(include/rtk_asensing)
 
-add_executable(rtk_asensing_node src/rtk_asensing_node.cpp src/serial_read.cpp)
+add_executable(rtk_asensing_node src/rtk_asensing_node.cpp)
-target_link_libraries(rtk_asensing_node ${Boost_LIBRARIES})
 ament_target_dependencies(
   rtk_asensing_node
   rclcpp
   std_msgs
   sweeper_interfaces
-  logger)
+  logger
+  imu_msgs)
 
 install(TARGETS rtk_asensing_node DESTINATION lib/${PROJECT_NAME})
 

src/perception/rtk_asensing/config/rtk_asensing_params.yaml

@@ -1,6 +0,0 @@
-rtk_asensing:
-  ros__parameters:
-    # 串口设备名称
-    serial_port: "/dev/ttyTHS1"
-    # 波特率
-    baud_rate: 460800

src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp

@@ -1,23 +0,0 @@
-#ifndef __UART_READ_H__
-#define __UART_READ_H__
-
-#include <boost/asio.hpp>
-#include <iostream>
-
-using namespace std;
-using namespace boost::asio;
-
-class Boost_serial
-{
-   public:
-    Boost_serial();
-    ~Boost_serial();
-    void init(const string port_name, int baud_rate = 115200);
-    int serial_read(char buf[], int size);
-
-   private:
-    io_service service;
-    serial_port* sp;
-};
-
-#endif

src/perception/rtk_asensing/launch/rtk_asensing.launch.py

@@ -1,24 +1,14 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
 
 
 def generate_launch_description():
-    # 获取配置文件路径
-    config_dir = os.path.join(
-        get_package_share_directory("rtk_asensing"),
-        "config",
-        "rtk_asensing_params.yaml",
-    )
-
     return LaunchDescription(
         [
             Node(
                 package="rtk_asensing",
                 executable="rtk_asensing_node",
                 name="rtk_asensing_node",
-                parameters=[config_dir],  # 从YAML文件加载参数
                 output="screen",
             ),
         ]

src/perception/rtk_asensing/src/rtk_asensing_node.cpp

@@ -1,10 +1,13 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include <cmath>
+
 #include "asensing_protocol.hpp"
+#include "imu_msgs/msg/gnss.hpp"
+#include "imu_msgs/msg/imu.hpp"
 #include "logger/logger.h"
 #include "rclcpp/rclcpp.hpp"
-#include "serial_read.hpp"
 #include "sweeper_interfaces/msg/rtk.hpp"
 
 class RtkAsensingNode : public rclcpp::Node
@@ -14,144 +17,61 @@ public:
     {
         LOG_INFO("%s节点已经启动.", name.c_str());
 
-        // 初始化串口读取
-        boost_serial = new Boost_serial();
-        std::string serial_port = "/dev/ttyTHS1";
-        int baud_rate = 115200;
-        this->declare_parameter("serial_port", serial_port);
-        this->declare_parameter("baud_rate", baud_rate);
-        this->get_parameter("serial_port", serial_port);
-        this->get_parameter("baud_rate", baud_rate);
-        boost_serial->init(serial_port.c_str(), baud_rate);
-
-        // 初始化协议解析器
-        protocol_parser = new AsensingProtocol();
-
         // 创建发布者
         rtk_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10);
 
-        // 创建定时器，100ms为周期，定时发布
+        // 订阅 /Ins topic
-        timer_ =
+        ins_sub_ = this->create_subscription<imu_msgs::msg::Imu>(
-            this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&RtkAsensingNode::timer_callback, this));
+            "/Ins", 10, std::bind(&RtkAsensingNode::ins_callback, this, std::placeholders::_1));
+
+        // 订阅 /Gnss topic (可选)
+        gnss_sub_ = this->create_subscription<imu_msgs::msg::Gnss>(
+            "/Gnss", 10, std::bind(&RtkAsensingNode::gnss_callback, this, std::placeholders::_1));
+
+        LOG_INFO("已订阅 /Ins 和 /Gnss topic");
     }
 
-    ~RtkAsensingNode()
+    ~RtkAsensingNode() {}
-    {
-        if (protocol_parser)
-        {
-            delete protocol_parser;
-        }
-        if (boost_serial)
-        {
-            delete boost_serial;
-        }
-    }
 
    private:
-    void timer_callback()
+    void ins_callback(const imu_msgs::msg::Imu::SharedPtr msg)
-    {
-        static int read_count = 0;
-        static int parse_count = 0;
-
-        try
-        {
-            // 读取串口数据
-            memset(serial_buf, 0, sizeof(serial_buf));
-            int num = boost_serial->serial_read(serial_buf, sizeof(serial_buf));
-
-            if (num <= 0)
-            {
-                return;
-            }
-
-            read_count++;
-
-            // 每100次打印一次原始数据
-            if (read_count % 100 == 1)
-            {
-                LOG_INFO("[DEBUG] 串口读取 %d 字节:", num);
-                int print_len = (num < 20) ? num : 20;
-                std::string hex_str;
-                for (int i = 0; i < print_len; i++)
-                {
-                    char hex[4];
-                    snprintf(hex, sizeof(hex), "%02X ", (uint8_t)serial_buf[i]);
-                    hex_str += hex;
-                }
-                LOG_INFO("[DEBUG] 前%d字节 HEX: %s", print_len, hex_str.c_str());
-            }
-
-            // 将数据送入协议解析器
-            int result = protocol_parser->feedData(reinterpret_cast<uint8_t*>(serial_buf), num);
-
-            if (result == 1)
-            {
-                parse_count++;
-                LOG_INFO("[DEBUG] 解析成功! 累计帧数: %d", parse_count);
-
-                // 成功解析一帧数据，发布ROS消息
-                publishData();
-            }
-        }
-        catch (const std::exception& e)
-        {
-            LOG_ERROR("定时器回调函数异常：%s", e.what());
-        }
-        catch (...)
-        {
-            LOG_ERROR("定时器回调函数未知异常");
-        }
-    }
-
-    void publishData()
     {
         try
         {
-            auto message = sweeper_interfaces::msg::Rtk();
+            auto rtk_msg = sweeper_interfaces::msg::Rtk();
 
-            // 获取解析后的数据
+            // 从 /Ins 消息中提取数据
-            double lat = protocol_parser->getLatitude();
+            rtk_msg.lat = msg->latitude;
-            double lon = protocol_parser->getLongitude();
+            rtk_msg.lon = msg->longitude;
-            double heading = protocol_parser->getYaw();
+            rtk_msg.head = msg->azimuth;
-            double speed = protocol_parser->getGroundSpeed();
+            rtk_msg.speed =
-
+                std::sqrt(msg->north_velocity * msg->north_velocity + msg->east_velocity * msg->east_velocity);
-            // 获取定位质量
+            rtk_msg.p_quality = msg->position_type;
-            int p_quality = protocol_parser->getPositionType();
+            rtk_msg.h_quality = msg->heading_type;
-            int h_quality = protocol_parser->getHeadingType();
-
-            message.lat = lat;
-            message.lon = lon;
-            message.head = heading;
-            message.speed = speed;
-            message.p_quality = p_quality;
-            message.h_quality = h_quality;
 
             LOG_INFO("Asensing RTK: lat:%.9f, lon:%.9f, heading:%.2f, speed:%.2f, p_quality:%d, h_quality:%d",
-                     message.lat, message.lon, message.head, message.speed, message.p_quality, message.h_quality);
+                     rtk_msg.lat, rtk_msg.lon, rtk_msg.head, rtk_msg.speed, rtk_msg.p_quality, rtk_msg.h_quality);
 
-            // 发布消息
+            rtk_publisher_->publish(rtk_msg);
-            rtk_publisher_->publish(message);
         }
         catch (const std::exception& e)
         {
-            LOG_ERROR("发布数据时发生异常：%s", e.what());
+            LOG_ERROR("ins_callback异常：%s", e.what());
         }
     }
 
-    // ROS定时器
+    void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg)
-    rclcpp::TimerBase::SharedPtr timer_;
+    {
+        // GNSS数据可用于补充RTK信息（如果需要）
+        // 当前实现中RTK消息主要从/Ins获取
+    }
 
     // ROS发布者
     rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_publisher_;
 
-    // 串口读取类指针
+    // ROS订阅者
-    Boost_serial* boost_serial;
+    rclcpp::Subscription<imu_msgs::msg::Imu>::SharedPtr ins_sub_;
-
+    rclcpp::Subscription<imu_msgs::msg::Gnss>::SharedPtr gnss_sub_;
-    // 协议解析器指针
-    AsensingProtocol* protocol_parser;
-
-    // 串口读取buffer
-    char serial_buf[300];
 };
 
 int main(int argc, char** argv)

src/perception/rtk_asensing/src/serial_read.cpp

@@ -1,102 +0,0 @@
-#include "serial_read.hpp"
-
-#include "logger/logger.h"
-
-Boost_serial::Boost_serial() : sp(nullptr) { ; }
-
-Boost_serial::~Boost_serial()
-{
-    if (sp)
-    {
-        try
-        {
-            if (sp->is_open())
-            {
-                sp->close();
-            }
-            delete sp;
-        }
-        catch (...)
-        {
-            LOG_ERROR("关闭串口时发生异常");
-        }
-    }
-}
-
-void Boost_serial::init(const string port_name, int baud_rate)
-{
-    try
-    {
-        sp = new serial_port(service);
-        sp->open(port_name);
-
-        if (sp->is_open())
-        {
-            sp->set_option(serial_port::baud_rate(baud_rate));
-            sp->set_option(serial_port::flow_control(serial_port::flow_control::none));
-            sp->set_option(serial_port::parity(serial_port::parity::none));
-            sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
-            sp->set_option(serial_port::character_size(8));
-            LOG_INFO("打开串口成功！");
-        }
-        else
-        {
-            LOG_ERROR("打开串口失败！");
-            delete sp;
-            sp = nullptr;
-        }
-    }
-    catch (const exception& e)
-    {
-        LOG_ERROR("初始化串口时发生异常：%s", e.what());
-        if (sp)
-        {
-            delete sp;
-            sp = nullptr;
-        }
-    }
-    catch (...)
-    {
-        LOG_ERROR("初始化串口时发生未知异常");
-        if (sp)
-        {
-            delete sp;
-            sp = nullptr;
-        }
-    }
-}
-
-int Boost_serial::serial_read(char buf[], int size)
-{
-    if (sp == nullptr || !sp->is_open())
-    {
-        LOG_WARN("串口未初始化或未打开");
-        return -1;
-    }
-
-    try
-    {
-        int num = read(*sp, buffer(buf, size));
-        if (num < 0)
-        {
-            LOG_WARN("串口读取错误");
-            return -1;
-        }
-        else if (num == 0)
-        {
-            LOG_INFO("串口无数据可读");
-            return 0;
-        }
-        return num;
-    }
-    catch (const exception& e)
-    {
-        LOG_ERROR("串口读取异常：%s", e.what());
-        return -1;
-    }
-    catch (...)
-    {
-        LOG_ERROR("串口读取未知异常");
-        return -1;
-    }
-}