Ubuntu学习笔记6-ESP32接收并处理cmd_vel话题

Ubuntu学习笔记6-ESP32接收并处理cmd_vel话题

前言

前面配置好了Arduino环境中的ros库，但是经过各种尝试总结出以下结论：
rosserial_Arduino的串口通讯只支持Arduino系列板子（ESP8266没有尝试），当将串口通讯的代码烧录至ESP32后，每次通讯时都会报错：

Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

或者是另外一个M开头的报错，究其本质，感觉就是该库的串口对于ESP系列的不支持，由于本人技术有限，无法进行源码的修改及优化（当然也有可能是我菜没弄好，有懂行的大佬欢迎指点）
因此，根据网上的资料，本人尝试了另外一种通讯方式，通过ROS官方封装的TCP协议，将ESP板子接入该服务器中，即可通过局域网进行无线通讯，该方案有好也有坏。
好处：无需连接，可以直接按照ROS的节点方式进行编写ESP32端代码
缺点：不稳定，有时候连接异常，且数据通讯质量无法保证（估计受网络带宽等影响）
因此，随后还是放弃了这种方式，选择了直接通过串口库进行开发，本来想在Qt上开发ROS利用Qt的串口库进行通讯，但是冲浪发现ROS也有一个serial
库，类似封装Linux的串口库的产物，因此决定采用这种方式进行通讯。

ESP32端测试代码

对于ESP32而言，只需要简单的串口接收然后通过另外一个串口输出即可验证，主要的处理还是在上位机ROS端。
ESP32的串口0（该串口用于下载，使用了USB线就不能再用杜邦线接出）

TXD：GPIO1
RXD：GPIO3

ESP32的串口2

TXD：GPIO17
RXD：GPIO16

连接方式如上。
ESP32代码如下：

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial2.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available())
  {
    String buf = Serial.readString();
    Serial2.println(buf);
  }
}

ROS端代码

ROS端需要订阅键盘节点话题（/cmd_vel），并将接收到的线速度及角速度转化为转速，然后通过串口下发给ESP32.

#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <sstream>
using namespace std;

//定义串口对象
serial::Serial ser;

//cmd_vel回调函数，用于接收键盘话题下发ESP32
void com_vel_callback(const geometry_msgs::Twist::ConstPtr& msg){
    int leftSpeed = (msg->linear.x - msg->angular.z * 0.27 / 2) / 3.14 / 0.085 *100;
    int rightSpeed = (msg->linear.x + msg->angular.z *0.27 / 2) / 3.14 / 0.085 *100;

    std_msgs::String buf;
    std::stringstream ss;
    ss<<leftSpeed<<","<<rightSpeed;
    buf.data = ss.str();
    ser.write(buf.data);
}

int rosSerial_init(string name,int baud)
{
    try
    {
        ser.setPort(name);
        ser.setBaudrate(baud);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }

    if(ser.isOpen()){
        ROS_INFO_STREAM("Serial Port initialized");
    }else{
        return -1;
    }
}

int main (int argc, char** argv){
    ros::init(argc, argv, "serial_example_node");
    ros::NodeHandle nh;

    ros::Subscriber write_sub = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 1000, com_vel_callback);
    ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);

    if(rosSerial_init("/dev/ttyUSB0",115200) == -1)
        return -1;

    ros::Rate loop_rate(5);
    while(ros::ok()){

        ros::spinOnce();

        if(ser.available()){
            ROS_INFO_STREAM("Reading from serial port");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            ROS_INFO_STREAM("Read: " << result.data);
            read_pub.publish(result);
        }
        loop_rate.sleep();

    }
}

关于线速度及角速度转化为转速的知识自行冲浪补习，此处提供下位机的数据处理方法，以供参考：

  target[0] = 0;target[1] = 0; 
  String buf = str.data;
  char com[20];
  buf.toCharArray(com,10);
  int m = 0;
  for(int i = 0;i<20;i++)
  {
    if(com[i] == ',')
    {
      if(flag_1 == 1)
        target[m] *= -1;
      flag_1 = 0;   
      m++;
    }
    else if(com[i] == '\r'||com[i] == '\n'||com[i] == '\0')
      break;
    else
    {
      if(com[i] != '-')
        target[m] = target[m]*10+(com[i]-'0');
      else
        flag_1 = 1;    
    }
  }
  if(flag_1 == 1)
  {
    target[1] *= -1;
    flag_1 = 0;
  }
  target[0] /= 100;target[1] /= 100;
  if(target[0] >= 0)   Lpid.SetOutputLimits(500, 1000);
  else                Lpid.SetOutputLimits(-1000, -500);
  if(target[1] >= 0)   Rpid.SetOutputLimits(500, 1000);
  else                Rpid.SetOutputLimits(-1000, -500);
  Serial.print(target[0]);
  Serial.print(',');
  Serial.println(target[1]);

实现效果

PC的串口助手成功接收上位机下发的转速信息

后记

尽管这种方法能够接收ROS下发的控制信息，但是ESP32端的odam及里程计信息等如何上传还需要研究一下，希望一切顺利。