Ubuntu学习笔记6-ESP32接收并处理cmd

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');elseflag_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及里程计信息等如何上传还需要研究一下，希望一切顺利。