diff --git a/src/perception/rtk_asensing/CMakeLists.txt b/src/perception/rtk_asensing/CMakeLists.txt index 25dfc13..fda6a9f 100644 --- a/src/perception/rtk_asensing/CMakeLists.txt +++ b/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) -target_link_libraries(rtk_asensing_node ${Boost_LIBRARIES}) +add_executable(rtk_asensing_node src/rtk_asensing_node.cpp) ament_target_dependencies( rtk_asensing_node rclcpp std_msgs sweeper_interfaces - logger) + logger + imu_msgs) install(TARGETS rtk_asensing_node DESTINATION lib/${PROJECT_NAME}) diff --git a/src/perception/rtk_asensing/config/rtk_asensing_params.yaml b/src/perception/rtk_asensing/config/rtk_asensing_params.yaml deleted file mode 100644 index 4f6afde..0000000 --- a/src/perception/rtk_asensing/config/rtk_asensing_params.yaml +++ /dev/null @@ -1,6 +0,0 @@ -rtk_asensing: - ros__parameters: - # 串口设备名称 - serial_port: "/dev/ttyTHS1" - # 波特率 - baud_rate: 460800 diff --git a/src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp b/src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp deleted file mode 100644 index 7e0701b..0000000 --- a/src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef __UART_READ_H__ -#define __UART_READ_H__ - -#include -#include - -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 diff --git a/src/perception/rtk_asensing/launch/rtk_asensing.launch.py b/src/perception/rtk_asensing/launch/rtk_asensing.launch.py index 3d97cd3..a4ee952 100644 --- a/src/perception/rtk_asensing/launch/rtk_asensing.launch.py +++ b/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", ), ] diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp index a32e9cf..c94f8c9 100644 --- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp +++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp @@ -1,157 +1,77 @@ #include #include +#include + #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 { -public: + public: RtkAsensingNode(std::string name) : Node(name) { 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("rtk_message", 10); - // 创建定时器,100ms为周期,定时发布 - timer_ = - this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&RtkAsensingNode::timer_callback, this)); + // 订阅 /Ins topic + ins_sub_ = this->create_subscription( + "/Ins", 10, std::bind(&RtkAsensingNode::ins_callback, this, std::placeholders::_1)); + + // 订阅 /Gnss topic (可选) + gnss_sub_ = this->create_subscription( + "/Gnss", 10, std::bind(&RtkAsensingNode::gnss_callback, this, std::placeholders::_1)); + + LOG_INFO("已订阅 /Ins 和 /Gnss topic"); } - ~RtkAsensingNode() - { - if (protocol_parser) - { - delete protocol_parser; - } - if (boost_serial) - { - delete boost_serial; - } - } + ~RtkAsensingNode() {} -private: - void timer_callback() - { - 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(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() + private: + void ins_callback(const imu_msgs::msg::Imu::SharedPtr msg) { try { - auto message = sweeper_interfaces::msg::Rtk(); + auto rtk_msg = sweeper_interfaces::msg::Rtk(); - // 获取解析后的数据 - double lat = protocol_parser->getLatitude(); - double lon = protocol_parser->getLongitude(); - double heading = protocol_parser->getYaw(); - double speed = protocol_parser->getGroundSpeed(); - - // 获取定位质量 - int p_quality = protocol_parser->getPositionType(); - 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; + // 从 /Ins 消息中提取数据 + rtk_msg.lat = msg->latitude; + rtk_msg.lon = msg->longitude; + rtk_msg.head = msg->azimuth; + rtk_msg.speed = + std::sqrt(msg->north_velocity * msg->north_velocity + msg->east_velocity * msg->east_velocity); + rtk_msg.p_quality = msg->position_type; + rtk_msg.h_quality = msg->heading_type; 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(message); + rtk_publisher_->publish(rtk_msg); } catch (const std::exception& e) { - LOG_ERROR("发布数据时发生异常:%s", e.what()); + LOG_ERROR("ins_callback异常:%s", e.what()); } } - // ROS定时器 - rclcpp::TimerBase::SharedPtr timer_; + void gnss_callback(const imu_msgs::msg::Gnss::SharedPtr msg) + { + // GNSS数据可用于补充RTK信息(如果需要) + // 当前实现中RTK消息主要从/Ins获取 + } // ROS发布者 rclcpp::Publisher::SharedPtr rtk_publisher_; - // 串口读取类指针 - Boost_serial* boost_serial; - - // 协议解析器指针 - AsensingProtocol* protocol_parser; - - // 串口读取buffer - char serial_buf[300]; + // ROS订阅者 + rclcpp::Subscription::SharedPtr ins_sub_; + rclcpp::Subscription::SharedPtr gnss_sub_; }; int main(int argc, char** argv) diff --git a/src/perception/rtk_asensing/src/serial_read.cpp b/src/perception/rtk_asensing/src/serial_read.cpp deleted file mode 100644 index 79528b5..0000000 --- a/src/perception/rtk_asensing/src/serial_read.cpp +++ /dev/null @@ -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; - } -}