lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

rtk_asensing

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-29 14:22:23 +08:00
parent 608f794be7
commit 9c6673c0a2
6 changed files with 41 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/perception/rtk_asensing/CMakeLists.txt
View File

@ -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})

6
src/perception/rtk_asensing/config/rtk_asensing_params.yaml
View File

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

23
src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp
View File

@ -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

10
src/perception/rtk_asensing/launch/rtk_asensing.launch.py
View File

@ -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",
),
]

154
src/perception/rtk_asensing/src/rtk_asensing_node.cpp
View File

@ -1,157 +1,77 @@
#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
{
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<sweeper_interfaces::msg::Rtk>("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<imu_msgs::msg::Imu>(
"/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()
{
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<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()
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<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_publisher_;
// 串口读取类指针
Boost_serial* boost_serial;
// 协议解析器指针
AsensingProtocol* protocol_parser;
// 串口读取buffer
char serial_buf[300];
// ROS订阅者
rclcpp::Subscription<imu_msgs::msg::Imu>::SharedPtr ins_sub_;
rclcpp::Subscription<imu_msgs::msg::Gnss>::SharedPtr gnss_sub_;
};
int main(int argc, char** argv)

102
src/perception/rtk_asensing/src/serial_read.cpp
View File

@ -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;
}
}