ROS2Foxy和ArduinoESP32消息和速度指令发布调试笔记

ROS2对节点信息稳定性有要求，如果用wifi等节点间时延过长会导致无法成功，部分路由器也会有不兼容情况（DDS大法好奇妙），请一定注意！

 手机热点IP和时延：

手机热点易出现无法连接成功的情况！DDS累觉不爱了……不爽了！一定要快，快，快！！！

路由器IP和时延：

使用路由器稳定可靠，路由器越神奇（DDS爱它），ros2消息发布接收越稳定，就是容易成功建立连接。

这样效果，代码如下：

  
  

   
   

    
    

     
     

    
    
    
    

     
     

      #include <ros2arduino.h>
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     
 
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      #include <WiFi.h>
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      #include <WiFiUdp.h>
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      #define SSID "****"
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      #define SSID_PW "****"
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      #define AGENT_IP "****"
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      #define AGENT_PORT 2020 //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);
     
     
    
    
   
   

    
    

     
     

    
    
    
    

     
     

      
      }
     
     
    
    
  
  
 
 

具体调试过程全部录屏了，看视频跟着做就好啦。

消息发布测试：

速度指令发布测试：

这段视频的最后将问题暴露出来，请注意！时延大于50ms以上无法建立可靠连接的！

分析和解决方法，更换路由器或者重启手机热点：

彩蛋在视频中，“悟”

（turtlesim版）

~Fin~

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/109566819