【24电赛飞控】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" >> ~/.bashrc

4.运行测试

打开终端

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博客