代码收藏家 【电赛飞控】T265与ROS通信详解：节点串口与飞控（stm32，msp432）交互指南
在上一篇内容中完成了T265的Ubuntu20.04环境构建与ROS安装，故现在我们需要将T265与ROS以及STM32链接起来形成完整的信息通路，以达到平替UWB系统来提供绝对坐标的效果
T265与ros节点
在RealsenseSDK中，已经提供了例程可以得到T265测量的绝对坐标值
我们只需要使用rossense2-camera节点发布，然后创建一个节点进行订阅再由串口发出即可
以下内容参考自五、ROS学习之订阅T265里程计数据并与stm32通信_t265与stm32无人机-CSDN博客
roslaunch realsense2_camera demo_t265.launch
运行这个可以查看rviz内的位姿信息则环境配置正确
roslaunch realsense2_camera rs_t265.launch
运行这个可以让ros在camera/odom/sample话题（topic）下发布位姿信息
创建测试接受节点
在realsense2-camera包下创建一个文件stm32_sub_t265.cpp
如果你和我之前文章做的目录结构一样，那么该文件应该创建在~/catkin_ws/src/realsense-ros/realsense2-camera/src下，如图
1. stm32_sub_t265.cpp内写入如下内容
#include "../include/t265_realsense_node.h"
#include "t265_realsense_node.h"
#include <serial/serial.h>
void doMsg(const nav_msgs::Odometry::ConstPtr& msg)
{
float pose_x=0,pose_y=0,pose_z=0,speed_x=0,speed_y=0,speed_z=0;
pose_x = -msg->pose.pose.position.x;
pose_y = -msg->pose.pose.position.y;
pose_z = msg->pose.pose.position.z;
speed_x = msg->twist.twist.linear.x;
speed_y = -msg->twist.twist.linear.y;
speed_z = msg->twist.twist.linear.z;
ROS_INFO("X_坐标：%f Y_坐标：%f Z_坐标：%f", pose_x,pose_y,pose_z);
ROS_INFO("X_速度：%f Y_速度：%f Z_速度：%f", speed_x,speed_y,speed_z);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "air_t265");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/camera/odom/sample",10,doMsg);
ros::spin();
return 0;
}
可见程序引用了Serial.h，通过apt安装这个ROS包
sudo apt install ros-noetic-serial
其中，noetic需要替换成你的ROS版本代号
2.在CMakeList.txt中添加如下代码
注意，是realsense2-camera内的内容，不是外部带锁的CMakelist，外部的如果修改将不能通过编译。
add_executable(stm32_sub_t265 src/stm32_sub_t265.cpp)
target_link_libraries(stm32_sub_t265
${catkin_LIBRARIES}
)
并在add-library下添加内容src/stm32_sub_t265.cpp
与如下一致
add_library(${PROJECT_NAME}
include/constants.h
include/realsense_node_factory.h
include/base_realsense_node.h
include/t265_realsense_node.h
src/realsense_node_factory.cpp
src/base_realsense_node.cpp
src/t265_realsense_node.cpp
src/stm32_sub_t265.cpp
)
3.编译realsen2-camera包
cd ~/catkin_ws
catkin_make
catkin_make install添加环境变量
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc4.运行测试
打开终端
roscore如果终端提示找不到节点，输入
source ~/.bashrc插入T265，另开终端
roslaunch realsense2_camera rs_t265.launch运行t265位姿发布程序
再次另开一个终端
roslaunch realsense2_camera stm32_sub_t265
得出位姿数据
创建串口接受发送节点
修改stm32_sub_t265.cpp
#include "../include/t265_realsense_node.h"
#include <serial/serial.h>
serial::Serial ros_ser;
void Send_Odem(float x,float y,float z,float vx,float vy,float vz);
void doMsg(const nav_msgs::Odometry::ConstPtr& msg)
{
float pose_x=0,pose_y=0,pose_z=0,speed_x=0,speed_y=0,speed_z=0;
pose_x = -msg->pose.pose.position.x;
pose_y = -msg->pose.pose.position.y;
pose_z = msg->pose.pose.position.z;
speed_x = msg->twist.twist.linear.x;
speed_y = -msg->twist.twist.linear.y;
speed_z = msg->twist.twist.linear.z;
Send_Odem(pose_x,pose_y,pose_z,speed_x,speed_y,speed_z);
ROS_INFO("X_坐标：%f Y_坐标：%f Z_坐标：%f", pose_x,pose_y,pose_z);
ROS_INFO("X_速度：%f Y_速度：%f Z_速度：%f", speed_x,speed_y,speed_z);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "air_t265");
ros::NodeHandle nh;
try
{
ros_ser.setPort("/dev/ttyUSB0");
ros_ser.setBaudrate(115200);
//ros_serial::Timeout to = serial::Timeout::simpleTimeout(1000);
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
ros_ser.setTimeout(to);
ros_ser.open();
ros_ser.flushInput(); //清空缓冲区数据
}
catch (serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port ");
return -1;
}
if(ros_ser.isOpen())
{
ros_ser.flushInput(); //清空缓冲区数据
ROS_INFO_STREAM("Serial Port opened");
}
else
{
return -1;
}
ros::Rate loop_rate(100);
ros::Subscriber sub = nh.subscribe("/camera/odom/sample",10,doMsg);
ros::spin();
return 0;
}
void Send_Odem(float x,float y,float z,float vx,float vy,float vz)
{
uint8_t data_tem[50];
unsigned char i,counter=0;
unsigned char cmd;
unsigned int check=0;
cmd =0xF2;
data_tem[counter++] =0xAE;
data_tem[counter++] =0xEA;
data_tem[counter++] =0x0B;
data_tem[counter++] =cmd;
data_tem[counter++] =x*1000/256; // X
data_tem[counter++] =x*1000;
data_tem[counter++] =y*1000/256; // X
data_tem[counter++] =y*1000;
data_tem[counter++] =z*1000/256; // X
data_tem[counter++] =z*1000;
data_tem[counter++] =vx*1000/256; // X
data_tem[counter++] =vx*1000;
data_tem[counter++] =vy*1000/256; // X
data_tem[counter++] =vy*1000;
data_tem[counter++] =vz*1000/256; // X
data_tem[counter++] =vz*1000;
data_tem[counter++] =0x00;
data_tem[counter++] =0x00;
for(i=0;i<counter;i++)
{
check+=data_tem[i];
}
data_tem[counter++] =0xff;
data_tem[2] =counter-2;
data_tem[counter++] =0xEF;
data_tem[counter++] =0xFE;
ros_ser.write(data_tem,counter);
//ros_ser.write(data_tem,counter);
}
此程序使用的是usbttl0串口，波特率115200，发送频率100hz
串口 ：Ubuntu关于串口的操作(查看串口信息、串口助手、串口权限)_ubuntu查看串口-CSDN博客
作者：赤霞浪客
