ROS2进阶 -- 硬件篇第五章第一节 -- 做个遥控车，让小车动起来 -- 订阅ROS2 Twist

一、新建工程

新建工程example1_ros2_car

修改配置

; PlatformIO Project Configuration File
;
;   Build options: build flags, source filter
;   Upload options: custom upload port, speed and extra flags
;   Library options: dependencies, extra library storages
;   Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
board_microros_transport = wifi  ; 指定使用的Micro-ROS传输方式为Wi-Fi
lib_deps =  ; 列出所有依赖库的URL，这些库将被下载和安装
	https://github.com/fishros/Esp32McpwmMotor.git  ; ESP32-MCPWM-Motor库，用于驱动电机
    https://gitee.com/ohhuo/micro_ros_platformio.git  ; Micro-ROS平台库，用于在ESP32上运行ROS 2

二、编写代码

#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <WiFi.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>

// 定义 ROS2 执行器和支持结构
rclc_executor_t executor;
rclc_support_t support;
// 定义 ROS2 内存分配器
rcl_allocator_t allocator;
// 定义 ROS2 节点和订阅者
rcl_node_t node;
rcl_subscription_t subscriber;
// 定义接收到的消息结构体
geometry_msgs__msg__Twist sub_msg;

// 定义控制两个电机的对象
Esp32McpwmMotor motor;

// 回调函数，当接收到新的 Twist 消息时会被调用
void twist_callback(const void *msg_in)
{
    
    
  // 将接收到的消息指针转化为 geometry_msgs__msg__Twist 类型
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  // 从 Twist 消息中获取线速度和角速度
  float linear_x = twist_msg->linear.x;
  float angular_z = twist_msg->angular.z;
  // 打印接收到的速度信息
  Serial.printf("recv spped(%f,%f)\n", linear_x, angular_z);
  // 如果速度为零，则停止两个电机
  if (linear_x == 0 && angular_z == 0)
  {
    
    
    motor.updateMotorSpeed(0, 0);
    motor.updateMotorSpeed(1, 0);
    return;
  }

  // 根据线速度和角速度控制两个电机的转速
  if (linear_x > 0)
  {
    
    
    motor.updateMotorSpeed(0, 70);
    motor.updateMotorSpeed(1, 70);
  }

  if (linear_x < 0)
  {
    
    
    motor.updateMotorSpeed(0, -70);
    motor.updateMotorSpeed(1, -70);
  }

  if (angular_z > 0)
  {
    
    
    motor.updateMotorSpeed(0, -70);
    motor.updateMotorSpeed(1, 70);
  }

  if (angular_z < 0)
  {
    
    
    motor.updateMotorSpeed(0, 70);
    motor.updateMotorSpeed(1, -70);
  }
}

void setup()
{
    
    
  // 初始化串口
  Serial.begin(115200);

  // 初始化两个电机的引脚
  motor.attachMotor(0, 12, 13);
  motor.attachMotor(1, 14, 27);

  // 设置 micro-ROS 通信参数，连接到指定的 ROS2 代理
  IPAddress agent_ip;
  agent_ip.fromString("192.168.43.235");
  set_microros_wifi_transports("LLLLLL", "Laj19980918.", agent_ip, 8888);
  delay(2000);

  // 初始化 ROS2 执行器和支持结构
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  // 初始化 ROS2 节点
  rclc_node_init_default(&node, "esp32_car", "", &support);
  // 初始化订阅者
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  // 设置订阅的回调函数
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);
}

void loop()
{
    
    
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); // 循环处理数据
}

代码使用 Esp32McpwmMotor 库初始化电机，设置 micro-ROS 通信参数以连接到 ROS2 代理，并初始化一个 ROS2 节点和一个订阅者，以订阅 /cmd_vel 主题上的 Twist 消息。

当接收到新的 Twist 消息时，调用 twist_callback() 函数提取线性和角速度，并相应地控制电机。如果两个速度都为零，则电机停止。否则，根据方向设置电机速度。在正向方向上，速度设置为 70，在反向方向上为 -70。

loop() 函数重复调用 rclc_executor_spin_some() 来处理来自 ROS2 网络的传入数据。

需要注意的是，你要根据自己的网络情况修改下面的代码以实现无线通信

agent_ip.fromString("192.168.2.105");
set_microros_wifi_transports("fishbot", "12345678", agent_ip, 8888);

三、下载测试

将代码下载到小车，运行agent，点击RST等待接入。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

接着我们使用ROS 2的键盘控制节点来进行控制测试

ros2 run teleop_twist_keyboard teleop_twist_keyboard

接着按键ijl，控制小车移动