NoopLoop ROS 节点 时间戳以及话题

1. 概述

阅读NoopLoop公司LinkTrack 系列UWB模块在ROS下节点源代码，将有用的部分总结一下，主要是时间戳以及topic的处理。

2. 说明

根据官方文档，LinkTrack模块可以执行三种不同的运行模式（LP(Local Positioning)局部定位，DR(Distributed Ranging)分布式测距，DT(DataTransmission)数据传输简称数传），每种模式下面又有具体的角色设置,本次主要研究LP模式，也就是局部定位模式。

2.1 LP模式角色

三种角色，
Tag：标签，需要定位的设备，相当于用户端的GNSS接收机。使用T0,T1,Ti区分不同设备。
Console：控制台，用于控制，相当于地面的监控站。可不需要。使用C0,C1,Ci区分不同控制台。
Anchor：基站，提供参考位置信息，相当于天空中的卫星。使用A0,A1,Ai区分不同基站。
定位原理：
根据Tag和Anchor之间的TOF，使用三边定位解算Tag的位置。
每种角色能够提供的信息不同，
进行配置时需要注意SystemCH|系统通道，SystemID|系统ID，TXGain|发射增益三个参数。同一系统中不同角色的参数保持一致。
对于LP模式，可以设置不同的模式LP_MODE0、LP_MODE1、、、LP_MODE6，不同模式下定位数据传输频率，数据传输频率会发生改变。
不同角色发布数据时使用的协议不一样，可以使用官方程序NAssiantant进行更改。我们设备的Tag输出数据协议为NODE_FRAME2，Anchor输出数据协议为NODE_FRAME1。协议不同输出的数据也会有差异，会影响到后面的协议解析程序。
大概就这些了，有需要再继续添加。

更多相关信息请参考官方文档。

3. 溯源

采用倒推的方式对LinkTrack系列产品在ROS下发布Topic进行溯源，理解源代码的运行过程。

1
运行下面的命令可以启动linktrack节点

$ roslaunch nlink_parser linktrack.launch

默认设备是/dev/ttyUSB0，波特率为921600

<launch>
    <node pkg="nlink_parser" type="linktrack" name="linktrack0" output="screen">
        <param name="port_name" value="/dev/ttyUSB0" />
        <param name="baud_rate" value="921600" />
    </node>
</launch>

2
首先主函数入口，定义串口类对象serial、初始类对象Init、帧数据提取类对象frameExtraction。

int main(int argc, char **argv) {
   
  ros::init(argc, argv, "linktrack_parser");
  ros::NodeHandle nh;
  serial::Serial serial;
  initSerial(&serial);
  NFrameExtraction frameExtraction;
  LinkTrack::Init linktrackInit(&frameExtraction, &serial);
  ros::Rate loopRate(1000);
  while (ros::ok()) {
   
    auto availableBytes = serial.available();
    std::string strReceived;
    if (availableBytes) {
   
      serial.read(strReceived, availableBytes);
      frameExtraction.unpackData(strReceived);
    }
    ros::spinOnce();
    loopRate.sleep();
  }
  return EXIT_SUCCESS;
}

3
初始化串口serial，头文件为

#include <serial/serial.h>

具体目录为

/usr/local/include/serial/serial.h

之前安装的串口库就提供了这个头文件以及其他库文件。安装的信息查看
应该试试使用这个串口类，看起来比较简洁。
串口设置就是这几个要素：
串口名，波特率，超时时间。
然后检测是否能够读取。
成功打开串口会在这里会输出两条提示信息。(总共也就三条提示信息)

ROS_INFO("try to open serial port with %s,%d", port_name.data(), baud_rate);
ROS_INFO("Serial port opened successfully, waiting for data.");

具体的串口设置代码如下

void initSerial(serial::Serial *serial) {
   
  try {
   

    auto port_name =
        ros::param::param<std::string>("~port_name", "/dev/ttyUSB0");
    auto baud_rate = ros::param::param<int>("~baud_rate", 921600);

    serial->setPort(port_name);
    serial->setBaudrate(static_cast<uint32_t>(baud_rate));
    ROS_INFO("try to open serial port with %s,%d", port_name.data(), baud_rate);
    auto timeout = serial::Timeout::simpleTimeout(10);
    // without setTimeout,serial can not write any data
    // https://stackoverflow.com/questions/52048670/can-read-but-cannot-write-serial-ports-on-ubuntu-16-04/52051660?noredirect=1#comment91056825_52051660
    serial->setTimeout(timeout);
    serial->open();

    if (serial->isOpen()) {