ROS2之ESP32简单速度消息测试（极限频率）

视频：

使用如下代码，测试：

#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>


#define SSID       "***"
#define SSID_PW    "***"
#define AGENT_IP   "***"
#define AGENT_PORT *** //AGENT port number

#define PUBLISH_FREQUENCY 5000 //hz

void publishVel(geometry_msgs::Twist* vel, void* arg)
{
  (void)(arg);
  static int cnt = 0;
  vel->linear.x = 0.1+0.01*cnt;  //线速度
  vel->angular.z = 0.1+0.01*cnt; //角速度
//  vel->linear.x = ((double)rand()/(RAND_MAX));  //随机线速度
//  vel->angular.z = ((double)rand()/(RAND_MAX)); //随机角速度
//  vel->linear.x = 0.2;                            //固定线速度
//  vel->angular.z = 1.0 - 0.001*cnt;               //变化角速度
  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);
}

void loop() 
{
  static VelPub VelNode;
  
  ros2::spin(&VelNode);
}

效果如下：

ros@ros:~$ ros2 topic -h
usage: ros2 topic [-h] [--include-hidden-topics]
                  Call `ros2 topic <command> -h` for more detailed usage. ...

Various topic related sub-commands

optional arguments:
  -h, --help            show this help message and exit
  --include-hidden-topics
                        Consider hidden topics as well

Commands:
  bw     Display bandwidth used by topic
  delay  Display delay of topic from timestamp in header
  echo   Output messages from a topic
  find   Output a list of available topics of a given type
  hz     Print the average publishing rate to screen
  info   Print information about a topic
  list   Output a list of available topics
  pub    Publish a message to a topic
  type   Print a topic's type

  Call `ros2 topic <command> -h` for more detailed usage.
 

ros@ros:~$ ros2 主题 -h
用法：ros2 topic [-h] [--include-hidden-topics]
                   调用 `ros2 topic <command> -h` 以获得更详细的用法。 ...

各种主题相关的子命令

可选参数：
   -h, --help 显示此帮助信息并退出
   --include-hidden-topics
                         也考虑隐藏的主题

命令：
   bw 显示主题使用的带宽
   delay 从标题中的时间戳显示主题的延迟
   echo 从一个主题输出消息
   find 输出给定类型的可用主题列表
   hz 将平均发布率打印到屏幕
   info 打印有关主题的信息
   list 输出可用主题的列表
   pub 向主题发布消息
   type 打印主题的类型

   调用 `ros2 topic <command> -h` 以获得更详细的用法。

ros@ros:~$ ros2 topic hz -h
usage: ros2 topic hz [-h] [--window WINDOW] [--filter EXPR] [--wall-time]
                     topic_name

Print the average publishing rate to screen

positional arguments:
  topic_name            Name of the ROS topic to listen to (e.g. '/chatter')

optional arguments:
  -h, --help            show this help message and exit
  --window WINDOW, -w WINDOW
                        window size, in # of messages, for calculating rate
                        (default: 10000)
  --filter EXPR         only measure messages matching the specified Python
                        expression
  --wall-time           calculates rate using wall time which can be helpful
                        when clock is not published during simulation
ros@ros:~$ 
 

ros@ros:~$ ros2 主题 hz -h
用法：ros2 topic hz [-h] [--window WINDOW] [--filter EXPR] [--wall-time]
                      主题名称

将平均发布率打印到屏幕上

位置参数：
   topic_name 要收听的 ROS 主题的名称（例如 '/chatter'）

可选参数：
   -h, --help 显示此帮助信息并退出
   --window 窗口，-w 窗口
                         窗口大小，以消息数为单位，用于计算速率
                         （默认值：10000）
   --filter EXPR 只测量匹配指定 Python 的消息
                         表达
   --wall-time 使用 wall time 计算速率，这很有帮助
                         在模拟期间未发布时钟时
ros@ros:~$