论ROS下IMU的串口通信节点

最新推荐文章于 2024-03-31 22:07:03 发布

鱼尾sama

最新推荐文章于 2024-03-31 22:07:03 发布

阅读量896

点赞数 1

分类专栏： C++ ROS 文章标签：自动驾驶

本文链接：https://blog.csdn.net/a850565178/article/details/111409986

版权

解决该问题主要有两种方法：

1. 中断法

在ARM中有IDLE中断这个概念，当串口发完消息，可以向主控芯片发送中断信号，进而进入中断处理程序，这样自然而然也就不会有丢帧问题发生。基于该思路，自然可以直接使用LINUX系统的接口来写程序，但我还是觉得能用库就用库吧。这里选择的是boost::asio::serial这个库，主要代码如下：

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "imu.hpp"
#include <Eigen/Geometry>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
using namespace std;
using namespace boost::asio;
IMU imuraw;
sensor_msgs::Imu imuros;

uint8_t buf[32];

void printall(std::vector<uint8_t> buffer);
int main(int argc, char **argv)
{
   
  ros::init(argc, argv, "CartIMU");
  ros::NodeHandle nh;
  ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("CartImu", 1000);
  io_service iosev;
  serial_port sp(iosev);
  try
  {
   
    sp.open("/dev/ttyUSB0");
  }
  catch (const std::exception &e)
  {
   
    ROS_ERROR_STREAM(e.what() << ". please plugin in the USB of IMU.");
  }
  sp.set_option(serial_port::baud_rate(115200));
  sp.set_option(serial_port::flow_control(serial_port::flow_control::none)); //无流控制
  sp.set_option(serial_port::parity(serial_port::parity::none));             //无校验
  sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));        //1位停止位
  sp.set_option(serial_port::character_size(8));
  ros::Rate loop_rate(100);
  Eigen::Matrix3f rotation;
  Eigen::Quaternionf q;
  //不变的消息内容事先设置，降低运算量
  imuros.header.frame_id = "cart_imu";
  imuros.angular_velocity_covariance[0] = 0.013;
  imuros.angular_velocity_covariance[1] = 0;
  imuros.angular_velocity_covariance[2] = 0;
  imuros.angular_velocity_covariance[3] = 0;
  imuros.angular_velocity_covariance[4] = 0.013;
  imuros.angular_velocity_covariance[5] = 0;
  imuros.angular_velocity_covariance[6] = 0;
  imuros.angular_velocity_covariance[7] = 0;
  imuros.angular_velocity_covariance[8] = 0.013;