如何使用esp32从零制作一个ROS2的teleop遥控器（cmd_vel）

柔情只为你懂 2022-09-16 11:27 9阅读 0赞

使用课程所用镜像，硬件连线如下图所示：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16][]

使用io口34和36，对应的arduino端口如下列表：

static const uint8_t A0 = 36;
    static const uint8_t A3 = 39;
    static const uint8_t A4 = 32;
    static const uint8_t A5 = 33;
    static const uint8_t A6 = 34;
    static const uint8_t A7 = 35;
    static const uint8_t A10 = 4;
    static const uint8_t A11 = 0;
    static const uint8_t A12 = 2;
    static const uint8_t A13 = 15;
    static const uint8_t A14 = 13;
    static const uint8_t A15 = 12;
    static const uint8_t A16 = 14;
    static const uint8_t A17 = 27;
    static const uint8_t A18 = 25;
    static const uint8_t A19 = 26;

分别是A0和A6，测试一下，是否ok。

void setup() {
      // initialize serial communication at 9600 bits per second:
      Serial.begin(9600);
    }
    
    // the loop routine runs over and over again forever:
    void loop() {
      // read the input on analog pin 0:
      int sensorValue = analogRead(A0);
      // print out the value you read:
      Serial.println(sensorValue);
      delay(1);        // delay in between reads for stability
    }

分别测一下A0和A6是否正常。

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 1][]

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 2][]

参考如下两篇：

[esp32(ROS2foxy)从字符串发布到速度发布turtlesim\_zhangrelay的专栏-CSDN博客][esp32_ROS2foxy_turtlesim_zhangrelay_-CSDN]

[esp32➡遥控篇➡turtlesim➡mobot➡turtlebot3\_zhangrelay的专栏-CSDN博客][esp32_turtlesim_mobot_turtlebot3_zhangrelay_-CSDN]

然后尝试写出如下代码：

#include <ros2arduino.h>
     
    #include <WiFi.h>
    #include <WiFiUdp.h>
    #define SSID       "***"
    #define SSID_PW    "***"
    #define AGENT_IP   "***"
    #define AGENT_PORT 2021 //AGENT port number
     
    #define PUBLISH_FREQUENCY 2 //hz
    
    int linx_mid = 0;
    int angz_mid = 0;
    
    int sensorlinx = 0;
    int sensorangz = 0;
     
    void publishVel(geometry_msgs::Twist* vel, void* arg)
    {
      (void)(arg);
      static int cnt = 0;
      vel->linear.x = (sensorlinx-linx_mid)/1024.0;  //线速度
      vel->angular.z = (sensorangz-angz_mid)/1024.0; //角速度
      cnt++;
    }
     
    class VelPub : public ros2::Node
    {
    public:
      VelPub()
      : Node("esp32_cmdvel")
      {
        ros2::Publisher<geometry_msgs::Twist>* publisher_ = this->createPublisher<geometry_msgs::Twist>("turtle1/cmd_vel");
        this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishVel, nullptr, publisher_);
      }
    };
     
    WiFiUDP udp;
     
    void setup() 
    {
      WiFi.begin(SSID, SSID_PW);
      while(WiFi.status() != WL_CONNECTED);
     
      ros2::init(&udp, AGENT_IP, AGENT_PORT);
      linx_mid = analogRead(A6);
      angz_mid = analogRead(A0);
    }
     
    void loop() 
    {
      
      sensorlinx = analogRead(A6);
      sensorangz = analogRead(A0);
      static VelPub VelNode;
      
      ros2::spin(&VelNode);
    }

需要配置Micro-XRCE-DDS-Agent-1.3.0，在home文件下中新建ros/RobCode文件夹，并将编译好文件放入其中。

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 3][]

如果直接运行tool下工具会报错。需要安装此库。复制后即完成安装，如果不行，使用sudo make install。

正常后如下所示：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 4][]

先输入q退出，然后再输入如下命令行：

./MicroXRCEAgent udp4 --port 2021

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 5][]

完成配置。打开turtlesim。即可用esp32进行遥控。

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 6][]

--------------------

[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16]: /images/20220828/acf2d00930e44f888227e0b07892904f.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220828/d931bf29412d4002b84ab3cf1ddbd6b0.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220828/b08a9fb47f0a4355b5ed9066ded2de3c.png
[esp32_ROS2foxy_turtlesim_zhangrelay_-CSDN]: https://blog.csdn.net/ZhangRelay/article/details/120504387
[esp32_turtlesim_mobot_turtlebot3_zhangrelay_-CSDN]: https://blog.csdn.net/ZhangRelay/article/details/120538455
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 3]: /images/20220828/55cdec7d701745908a128033bce40eeb.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 4]: /images/20220828/a2b4b058479e419ba68dfc1e4153233e.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 5]: /images/20220828/467e1f0cfd7e40d8badac9fff9825ef2.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAemhhbmdyZWxheQ_size_20_color_FFFFFF_t_70_g_se_x_16 6]: /images/20220828/9264bfbda7a946b19353da381c8d0a88.png