五、ROS学习之订阅T265里程计数据并与stm32通信

通过节点订阅t265的里程计数据

我使用的是Kinetic版本
1）环境安装

sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo ap
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
sudo apt-get update
sudo apt-get install librealsense2-dkms librealsense2-utils librealsense2-dev librealsense2-dbg
realsense-viewer

成功运行的话就会出现Intel的上位机的界面了。
2）安装T265的ROS包

sudo apt-get install ros-kinetic-ddynamic-reconfigure
cd ~/catkin_ws/src/
git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1`
cd ..
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release

3)接下来运行这个可以在rviz中看到T265的图像信息

roslaunch realsense2_camera demo_t265.launch

运行这个可以使t265发布话题

roslaunch realsense2_camera rs_t265.launch

这样t265就可以发布数据了，我们以话题的方式来接受他

1.订阅T265的里程计信息

我用的是vscode开发环境

1)在realsense2_camera功能包里创建一个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;
}

2）在CMakeList.txt中添加如下代码

 add_executable(stm32_sub_t265 src/stm32_sub_t265.cpp)
 target_link_libraries(stm32_sub_t265
   ${catkin_LIBRARIES}
 )

并修改这段代码如下

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
    )


4）接下来就可以运行相关节点了
```javascript
roslaunch realsense2_camera rst265.launch

roslaunch realsense2_camera stm32_sub_t265

运行结果如下

2.实现linux系统与stm32通信，并向stm32发送里程计信息

1）将stm32_sub_t265中修改成如下代码

#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);
}

现在数据就通过串口发送出去了。
3）因为是在Jetson nano 中使用，所以我们需要设置成开机自动启动。
在catkin_ws中创建my_auto_start.sh文件，并编写如下代码

#! /bin/bash
### BEGIN INIT INFO
# Provides:          xxxx.com
# Required-Start:    $local_fs $network
# Required-Stop:     $local_fs
# Default-Start:     2 3 4 5
# Default-Stop:      0 1 6
# Short-Description: mylaunch service
# Description:       mylaunch service test
### END INIT INFO

gnome-terminal -x bash -c " cd catkin_ws/;source devel/setup.bash;roslaunch realsense2_camera rs_t265.launch" #新建终端启动节点
sleep 5 #等待2秒
gnome-terminal -x bash -c "cd catkin_ws/;source devel/setup.bash;echo '123456' | sudo -S cp file /etc/hosts;sudo chmod 666 /dev/ttyUSB0;rosrun realsense2_camera stm32_sub_t265" &  
wait
exit 0

具体请看这个博客
链接: ros节点开机自启动.
现在我们正常开机相关节点就会运行了。
4）接下来我们编写stm32相关代码