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

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;

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
}

#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);
}