Xsens ROS 节点时间戳以及话题

最新推荐文章于 2024-01-12 09:45:42 发布

Robots.

最新推荐文章于 2024-01-12 09:45:42 发布

阅读量2.9k

点赞数 1

分类专栏： IMU 文章标签： linux c++

本文链接：https://blog.csdn.net/i_robots/article/details/107208088

版权

IMU 专栏收录该内容

4 篇文章 2 订阅

订阅专栏

Xsens ROS 节点代码分析

1. 概述
2. 目录结构
3. 时间戳的处理
4. 延伸阅读

1. 概述

阅读Xsens在ROS下MTi系列节点源代码，将有用的部分总结一下，主要是时间戳以及topic的处理。

2. 目录结构

上一节对MTI系列的驱动和节点进行了处理，MTI_ROS_Driver。
节点目录结构如下

.
├── CMakeLists.txt
├── launch
│   ├── display.launch
│   └── xsens_mti_node.launch
├── lib
│   └── xspublic
│       ├── Makefile
│       ├── xscommon
│       │   ├── abstractadditionallogger.h
│       │   ├── 、、、、
│       ├── xscontroller
│       │   ├── broadcastdevice.cpp
│       │   └── 、、、、
│       └── xstypes
│           ├── datapacket_p.cpp
│           └── 、、、、
├── LICENSE.txt
├── package.xml
├── param
│   └── xsens_mti_node.yaml
├── README.txt
├── rviz
│   ├── display.rviz
│   └── example.rviz
├── src
│   ├── main.cpp
│   ├── messagepublishers
│   │   ├── accelerationpublisher.h
│   │   ├── angularvelocitypublisher.h
│   │   ├── freeaccelerationpublisher.h
│   │   ├── gnsspublisher.h
│   │   ├── imupublisher.h
│   │   ├── magneticfieldpublisher.h
│   │   ├── orientationincrementspublisher.h
│   │   ├── orientationpublisher.h
│   │   ├── packetcallback.h
│   │   ├── pressurepublisher.h
│   │   ├── temperaturepublisher.h
│   │   ├── timereferencepublisher.h
│   │   ├── transformpublisher.h
│   │   ├── twistpublisher.h
│   │   └── velocityincrementpublisher.h
│   ├── xdacallback.cpp
│   ├── xdacallback.h
│   ├── xdainterface.cpp
│   └── xdainterface.h
└── urdf
    ├── MTi_10.stl
    ├── MTi_10.urdf
    ├── MTi_1.stl
    ├── MTi_1.urdf
    ├── MTi_6xx.stl
    └── MTi_6xx.urdf

可以很清楚的看到launch文件所在的位置。

├── launch
│   ├── display.launch
│   └── xsens_mti_node.launch

3. 时间戳的处理

每一种传感器信息定义了一个结构体，具体的结构体信息可以查看每个文件。

│   ├── messagepublishers
│   │   ├── accelerationpublisher.h
│   │   ├── angularvelocitypublisher.h
│   │   ├── freeaccelerationpublisher.h
│   │   ├── gnsspublisher.h
│   │   ├── imupublisher.h
│   │   ├── magneticfieldpublisher.h
│   │   ├── orientationincrementspublisher.h
│   │   ├── orientationpublisher.h
│   │   ├── packetcallback.h
│   │   ├── pressurepublisher.h
│   │   ├── temperaturepublisher.h
│   │   ├── timereferencepublisher.h
│   │   ├── transformpublisher.h
│   │   ├── twistpublisher.h
│   │   └── velocityincrementpublisher.h

在构造函数中完成topic的初始化。
例如对IMU信息topic的处理如下，首先从参数服务器中获取发布消息队列大小，设置发布topic为imu/data，消息类型为sensor_msgs::Imu，这是标准的ROS消息类型，具体消息内容可以查看msg/Imu，进一步对方差进行了初始化。

    ImuPublisher(ros::NodeHandle &node)
    {
        int pub_queue_size = 5;
        ros::param::get("~publisher_queue_size", pub_queue_size);
        pub = node.advertise<sensor_msgs::Imu>("imu/data", pub_queue_size);

        // REP 145: Conventions for IMU Sensor Drivers (http://www.ros.org/reps/rep-0145.html)
        variance_from_stddev_param("~orientation_stddev", orientation_variance);
        variance_from_stddev_param("~angular_velocity_stddev", angular_velocity_variance);
        variance_from_stddev_param("~linear_acceleration_stddev", linear_acceleration_variance);
    }

因此，传感器发布信息的所有话题（topic）和消息类型如下

imu/data (sensor_msgs/Imu)
表示旋转平移的四元数，角速度，线加速度

imu/acceleration (geometry_msgs/Vector3Stamped)
校准线加速度

imu/angular_velocity (geometry_msgs/Vector3Stamped)
校准角速度

imu/mag (geometry_msgs/Vector3Stamped)
校准磁场

imu/dq (geometry_msgs/QuaternionStamped)
传感器的积分角速度（四元数表示）

imu/dv (geometry_msgs/Vector3Stamped)
传感器的积分线加速度

imu/time_ref (sensor_msgs/TimeReference)
设备时间戳

filter/quaternion (geometry_msgs/QuaternionStamped)
滤波四元数

filter/free_acceleration (geometry_msgs/Vector3Stamped)
滤波线加速度

filter/twist (geometry_msgs/TwistStamped)
速度和角速度

filter/positionlla (geometry_msgs/Vector3Stamped) (MTSS2019.3.2 and later)
滤波位置输出，包含经、纬、高

filter/velocity (MTSS2019.3.2 and later)
滤波速度输出

temperature (sensor_msgs/Temperature)
设备温度

pressure (sensor_msgs/FluidPressure)
设备气压

gnss (sensor_msgs/NavSatFix)
GNSS接收器的原始4 Hz数据，包含纬度，经度，高度和状态数据

tf (geometry_msgs/TransformStamped)
坐标变换

接下来将采用倒推的方式查看源代码，寻找时间戳定义位置以及总线数据的处理。
所有的publisher都存放在m_callbacks中，使用()运算进行新消息的发布，其实每一个publisher结构体都继承自callback结构体

struct ImuPublisher : public PacketCallback

因此可以使用()运算

void XdaInterface::spinFor(std::chrono::milliseconds timeout)
{
	RosXsDataPacket rosPacket = m_xdaCallback.next(timeout);

	if (!rosPacket.second.empty())
	{
		for (auto &cb : m_callbacks)
		{
			cb->operator()(rosPacket.second, rosPacket.first);
		}
	}
}

每次新数据的读取拥有固定的延时timeout，该值可以自己定义，在等待数据时加上了互斥锁。

// Returns empty packet on timeout
RosXsDataPacket XdaCallback::next(const std::chrono::milliseconds &timeout)
{
	RosXsDataPacket packet;

	std::unique_lock<std::mutex> lock(m_mutex);

	if (m_condition.wait_for(lock, timeout, [&] { return !m_buffer.empty(); }))
	{
		assert(!m_buffer.empty());

		packet = m_buffer.front();
		m_buffer.pop_front();
	}

	return packet;
}

具体的时间戳定义在此处，使用ROS时间来定义发布消息的时间。同样使用了互斥锁

void XdaCallback::onLiveDataAvailable(XsDevice *, const XsDataPacket *packet)
{
	std::unique_lock<std::mutex> lock(m_mutex);
	ros::Time now = ros::Time::now();

	assert(packet != 0);

	// Discard oldest packet if buffer full
	if (m_buffer.size() == m_maxBufferSize)
	{
		m_buffer.pop_front();
	}

	// Push new packet
	m_buffer.push_back(RosXsDataPacket(now, *packet));

	// Manual unlocking is done before notifying, to avoid waking up
	// the waiting thread only to block again
	lock.unlock();
	m_condition.notify_one();
}

在XsDevice::handleDataPacket()中进行了总线数据包的处理工作，调用了XdaCallback::onLiveDataAvailable()，考虑到数据的丢失时插值的情况。具体的代码太多就不贴了，可以自行查找源代码。

void XsDevice::handleDataPacket(const XsDataPacket &packet)

使用XsDevice::handleMessage()不同类型的总线信息进行处理，主要有MTData2、Error、Warning、MasterIndication等类型数据，我们所需要的数据属于MTData2类型，因此对这种类型的数据调用了XsDevice::handleDataPacket()。

void XsDevice::handleMessage(const XsMessage &msg)

以上就是本次内容，有新的发现会逐步添加。

4. 延伸阅读

关注

1
点赞
踩
16

收藏

觉得还不错? 一键收藏
6
评论
Xsens ROS 节点时间戳以及话题

Xsens ROS 节点代码分析概述目录结构时间戳的处理概述阅读Xsens在ROS下MTi系列节点源代码，将有用的部分总结一下，主要是时间戳以及topic的处理。目录结构上一节对MTI系列的驱动和节点进行了处理，MTI_ROS_Driver。节点目录结构如下.├── CMakeLists.txt├── launch│ ├── display.launch│ └── xsens_mti_node.launch├── lib│ └── xspublic│ ├─
复制链接

扫一扫