esp32和ros2之奔跑的turtlesim ?_?

#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "ESP32LoveROS2"
#define SSID_PW    "66666666"
#define AGENT_IP   "172.20.10.2"
#define AGENT_PORT 2021 //AGENT port number

#define PUBLISH_FREQUENCY 10 //hz

void publishString(geometry_msgs::Twist* msg, void* arg)
{
  (void)(arg);
  static int cnt = 0;
//  msg->linear.x = ((double)rand()/(RAND_MAX));  //随机线速度
//  msg->angular.z = ((double)rand()/(RAND_MAX)); //随机角速度
  msg->linear.x = 0.2;                            //固定线速度
  msg->angular.z = 1.0 - 0.001*cnt;               //变化角速度
  cnt++;
}

class StringPub : public ros2::Node
{
public:
  StringPub()
  : Node("esp32_cmdvel")
  {
    ros2::Publisher<geometry_msgs::Twist>* publisher_ = this->createPublisher<geometry_msgs::Twist>("turtle1/cmd_vel");
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishString, nullptr, publisher_);
  }
};

WiFiUDP udp;

void setup() 
{
  WiFi.begin(SSID, SSID_PW);
  while(WiFi.status() != WL_CONNECTED);

  ros2::init(&udp, AGENT_IP, AGENT_PORT);
}

void loop() 
{
  static StringPub StringNode;
  
  ros2::spin(&StringNode);
}

#define DEFAULT_BG_R 0xb5
#define DEFAULT_BG_G 0xc4
#define DEFAULT_BG_B 0xb1