ROS-UART-嵌入式设备之间数据读写

最新推荐文章于 2024-05-17 13:52:08 发布

纳百川@容苍穹

最新推荐文章于 2024-05-17 13:52:08 发布

阅读量667

点赞数

分类专栏： ROS通讯文章标签：单片机自动驾驶

本文链接：https://blog.csdn.net/qq_38877769/article/details/118308144

版权

ROS通讯专栏收录该内容

3 篇文章 0 订阅

订阅专栏

ROS-UART-单片机之间数据读写

ROS通过串口与嵌入式设备进行通信，总的来说有两种方式：
**第一种：**在嵌入式设备加载ROS库，让嵌入式设备成为一个节点，并发布话题，在ROS订阅该话题即可获得嵌入式设备的数据，同时嵌入式设备订阅ROS上的话题即可接收消息;
**第二种：**ROS应用层写一个与串口通讯的ROS节点，该节点负责从串口读取嵌入式系统传输给ROS应用层的数据，同时也负责将控制指令通过串口发送给嵌入系统系统，最终驱动实际的执行器去动作。
采用第二种方式：
项目代码如下：

#include "serial_node.h"
#include <iostream>
#include <string>
using namespace std;


int openSerialport(serial::Serial ros_ser,unsigned char* port,int baudrat)
{
    try {
        ros_ser.setPort(port.c_str());
        ros_ser.setBaudrat(baudrat);
        ros_ser::Timeout to=serial::Timeout::simpleTimeout(1000);
        ros_ser.setTimeout(to);
        ros_ser.open();
    }
    catch(serial::IOException& ioe)
    {
        ROS_INFO_STREAM("Unable to open serial port ");
        return 0;
    }
    if(ros_ser.isOpen())
        ROS_INFO_STREAM("serial port is opened");
    else
        return 0;
    return 1;
};

void recImudata(serial::Serial ros_ser,ros::NodeHandle& nh)
{
    ros::Rate loop_rate(200);
    ros::Publisher read_pub=nh.advertise<serial_node::my_msg>("read imu", 1000);
    while(ros::ok())
    {
        size_t n=ros_ser.available();
        if(n>35) {
            ros_ser.read(totalbuffer, n);
            unsigned char buf[30];
            int state = 0;
            int findpos = 0;
            for (int i = 0; i < buffer_size && i < n; i++) {
                if (state == 0 && totalbuffer[i] == 0xAA)
                    state = 1;
                else if (state == 1 && totalbuffer[i + 1] == 0xBB)
                    state = 2;
                else if (state == 2 && totalbuffer[i + 16] == 0xCC)
                    state = 3;
                else if (state == 3 && totalbuffer[i + 16] == 0xDD) {
                    findpos = i;
                    state = 4;
                    break;
                } else
                    state = 0;
            }
            if (state == 4) {
                for (int i = 0; i < 15; i++) {
                    buffer[i] = totalbuffer[findpos + i + 2];
                    buffer[i + 15] = totalbuffer[findpos + i + 18];
                }
                for (int k = 0; k < 6; k++) {
                    for (int j = 0; i < 4; j++)
                        _ga[k].data8[j] = buffer[k * 5 + j + 1];
                    _ros_msg.gyro_x = _ga[0].data;
                    _ros_msg.gyro_y = _ga[1].data;
                    _ros_msg.gyro_z = _ga[2].data;
                    _ros_msg.acc_x = _ga[3].data;
                    _ros_msg.acc_y = _ga[4].data;
                    _ros_msg.acc_z = _ga[5].data;
                    if (buffer[0] == 0xFE)
                        _ros_msg.gyro_x = -_ros_msg.gyro_x;
                    if (buffer[5] == 0xFE)
                        _ros_msg.gyro_y = -_ros_msg.gyro_y;
                    if (buffer[10] == 0xFE)
                        _ros_msg.gyro_z = -_ros_msg.gyro_z;
                    if (buffer[15] == 0xFE)
                        _ros_msg.acc_x = -_ros_msg.acc_x;
                    if (buffer[20] == 0xFE)
                        _ros_msg.acc_y = -_ros_msg.acc_y;
                    if (buffer[25] == 0xFE)
                        _ros_msg.acc_z = -_ros_msg.acc_z;
                }
                state = 0;
            }
        }
        read_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
};

void sendCallback(serial_node::my_msg msg,serial::Serial ser)
{
    IMU_data t[6];
    t[0].data=msg.gyro_x;
    t[1].data=msg.gyro_y;
    t[2].data=msg.gyro_z;
    t[3].data=msg.acc_x;
    t[4].data=msg.acc_y;
    t[5].data=msg.acc_z;
    _stm32_msg[0]=0xAA;
    _stm32_msg[1]=0xBB;
    for(int i=0;i<3;i++)
    {
        if (t[i].data < 0)
            _stm32_msg[2 + i * 5] = 0xFE;
        else
            _stm32_msg[2 + i * 5] = 0xFF;
        for (int j = 1; j < 5; j++)
            _stm32_msg[2 + i * 5 + j] = t[i].data8[j - 1];
    }
    _stm32_msg[2+3*5]=0xCC;
    for(int i=3;i<6;i++)
    {
        if (t[i].data < 0)
            _stm32_msg[2 + i * 5+1] = 0xFE;
        else
            _stm32_msg[2 + i * 5+1] = 0xFF;
        for (int j = 1; j < 5; j++)
            _stm32_msg[2 + i * 5 + j+1] = t[i].data8[j - 1];
    }
    _stm32_msg[2+5*6+4+2]=0xDD;

    ros_ser.write(_stm32_msg,34);
};

void sendImudata(ros::NodeHandle& nh)
{
    ros::Subscriber write_pub=nh.subscribe("write",34,sendCallback);
};

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"
#include "serial_node/my_msg.h"
using namespace std;
#define buffer_size 1000;
unsigned char totalbuffer[buffer_size];

typedef union
{
    float data;
    unsigned char data8[4];
} IMU_data;

IMU_data _ga[6];
serial_node::my_msg _ros_msg;  //接收到的数据
unsigned char _stm32_msg[34];  //发送给嵌入式设备的数据

int openSerialport(serial::Serial ros_ser,unsigned char* port,int baudrat);
void recImudata(serial::Serial ros_ser,ros::NodeHandle& nh);
void sendCallback(serial_node::my_msg msg,serial::Serial ros_ser);
void sendImudata(ros::NodeHandle& nh);