Jetson nano esp32实现避障小车（ROS2技术）

faiyun11

已于 2024-09-05 22:13:46 修改

阅读量1.3k

点赞数 14

文章标签： python c语言 vscode 人工智能网络协议 websocket

于 2024-09-04 18:06:44 首次发布

本文链接：https://blog.csdn.net/yunandfei/article/details/141899694

版权

一、实现功能

1、功能说明

利用以下硬件，制作一个智能小车（目前只有距离传感器硬件），实现通过ros2代码和ESP32代码，控制两个小电机的转速、转向，当接收到距离传感器返回的距离小于5厘米时，小车停止运行，并向左转向，直到检测到前方距离大于5厘米后继续向前行驶。

2.主要硬件设备

jetson nano，安装了ros2 humble，micro_ros，arduino，安装了一个摄像头CSI摄像头
ESP32 dev kit wroom-32U
L298N电机驱动板
直流减速小电机2个
超声波距离传感器

二、软硬件设计

1、硬件设计

Jetson Nano: 作为主控制器，运行ROS 2节点和micro_ros代理。处理摄像头图像、超声波传感器数据和其他传感器信息。执行高级算法，如计算机视觉和路径规划。通过ROS 2话题与ESP32通信。
ESP32: 作为微控制器，运行micro-ROS代理。直接控制L298N电机驱动板。接收Jetson Nano的指令来控制电机。
L298N电机驱动板: 用于驱动两个直流减速小电机。接收来自ESP32的PWM信号来控制电机速度和方向。
直流减速小电机: 为小车提供动力。可以配备编码器以反馈速度和位置信息。
超声波距离传感器: 用于检测小车前方的障碍物距离。

2、软件设计

ROS 2节点: 在Jetson Nano上运行，处理摄像头图像和超声波传感器数据。发布控制命令到ESP32。
micro_ros代理: 在Jetson Nano和ESP32上运行，实现两者之间的通信。
电机控制: ESP32订阅来自Jetson Nano的控制话题，根据接收到的指令控制L298N电机驱动板。
** 传感器数据处理:** ESP32收集超声波传感器数据，发送到Jetson Nano进行处理。
安全和监控: 实现紧急停止机制，以防止碰撞或应对其他潜在危险。

3、通信设计

使用ROS 2话题进行Jetson Nano和ESP32之间的数据交换。
通过无线网络实现Jetson Nano与外部系统的通信。（esp32应该连接wifi，本文暂未实现）

三、整体逻辑架构图

组件说明：

Jetson Nano：作为主控制器，运行ROS 2和micro_ros，处理摄像头图像和传感器数据，并通过ROS 2话题与ESP32通信。
ESP32：作为微控制器，运行micro-ROS代理，接收Jetson Nano的指令，控制L298N电机驱动板。
CSI摄像头：连接到Jetson Nano，用于捕捉环境图像，进行视觉处理。（这个只是设想，暂未实现）
L298N电机驱动板：接收来自ESP32的控制信号，驱动两个直流减速小电机。
直流减速小电机：连接到L298N电机驱动板，为小车提供动力。
超声波距离传感器：连接到ESP32，用于检测前方障碍物的距离。
电源：为Jetson Nano、ESP32、电机驱动板和传感器供电。
小车底盘：支撑和安装所有电子组件，包括电机、电机驱动板、传感器和ESP32。

组合方式：

Jetson Nano通过ROS 2话题与ESP32通信，发送控制指令。
CSI摄像头捕捉的图像直接传输给Jetson Nano进行处理。
ESP32根据Jetson Nano的指令，通过PWM信号控制L298N电机驱动板上的电机。
超声波距离传感器检测到的障碍物信息传输给ESP32，ESP32再将信息发送给Jetson Nano进行处理。
电源管理系统为所有电子组件提供稳定的电源。

四、实现代码

1、ROS 2 Python节点代码

这个节点将订阅距离传感器的话题，发布控制命令到ESP32。

import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from sensor_msgs.msg import Range

class MotorControlNode(Node):
    def __init__(self):
        super().__init__('motor_control_node')
        self.publisher_ = self.create_publisher(Int32MultiArray, 'motor_control', 10)
        self.subscription = self.create_subscription(
            Range,
            'distance_sensor',
            self.distance_callback,
            10)
        self.subscription  # prevent unused variable warning

    def distance_callback(self, msg):
        if msg.range < 0.05:  # 距离小于5厘米
            self.stop_motors()
            self.get_logger().info('Obstacle detected, stopping the car')
            self.execute_evasion_maneuver()
        else:
            self.forward_motors()

    def execute_evasion_maneuver(self):
        self.turn_left()
        self.wait_for_clearance()

    def wait_for_clearance(self):
        while True:
            msg = self.subscription.get_next_message()
            if msg.range > 0.05:
                self.forward_motors()
                break

    def stop_motors(self):
        msg = Int32MultiArray()
        msg.data = [0, 0]
        self.publisher_.publish(msg)

    def forward_motors(self):
        msg = Int32MultiArray()
        msg.data = [100, 100]  # 假设100为前进的速度
        self.publisher_.publish(msg)

    def turn_left(self):
        msg = Int32MultiArray()
        msg.data = [100, -100]  # 左电机前进，右电机后退
        self.publisher_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    motor_control_node = MotorControlNode()
    rclpy.spin(motor_control_node)
    motor_control_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2、ESP32 micro-ROS代码

这个代码将订阅来自ROS 2的控制命令，并根据这些命令控制电机。

#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32_multi_array.h>
#include <sensor_msgs/msg/range.h>

#include <rcl/error_handling.h>


std_msgs__msg__Int32MultiArray msg;
rcl_subscription_t subscriber;
rcl_publisher_t distance_publisher;
rclc_executor_t executor;
rclc_support_t support;
rcl_node_t node;
rcl_allocator_t allocator;

#define MOTOR_LEFT_FORWARD  5  // L298N IN1
#define MOTOR_LEFT_BACKWARD 18  // L298N IN2
#define MOTOR_RIGHT_FORWARD 23  // L298N IN3
#define MOTOR_RIGHT_BACKWARD 19  // L298N IN4
#define TRIGGER_PIN  4
#define ECHO_PIN     5
#define ENABLE_PIN   22  // L298N ENA

#define LED_PIN 13
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop(){
  while(1){
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}

void motor_control_callback(const void * msgin)
{
  const std_msgs__msg__Int32MultiArray * msg = (const std_msgs__msg__Int32MultiArray *)msgin;
  analogWrite(MOTOR_LEFT_FORWARD, msg->data.data[0]);
  analogWrite(MOTOR_LEFT_BACKWARD, msg->data.data[0]);
  analogWrite(MOTOR_RIGHT_FORWARD, msg->data.data[1]);
  analogWrite(MOTOR_RIGHT_BACKWARD, msg->data.data[1]);
  digitalWrite(ENABLE_PIN, msg->data.data[0] > 0 || msg->data.data[1] > 0 ? HIGH : LOW);
}

float measure_distance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration / 2) / 29.1; // 速度 = 34300 cm/s, 时间 = 距离 / (速度/2)
  return distance;
}

void setup() {
  float distance = measure_distance();
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(MOTOR_LEFT_FORWARD, OUTPUT);
  pinMode(MOTOR_LEFT_BACKWARD, OUTPUT);
  pinMode(MOTOR_RIGHT_FORWARD, OUTPUT);
  pinMode(MOTOR_RIGHT_BACKWARD, OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);

  digitalWrite(ENABLE_PIN, LOW);  // Disable motor driver initially

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "esp32_node", "", &support));

  // create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
    "motor_control"));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &distance_publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Range),
    "distance_sensor"));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  //
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &motor_control_callback, ON_NEW_DATA));
}

void loop() {

  //向ros2发布距离数据
  float distance = measure_distance();
  sensor_msgs__msg__Range distance_msg;
  distance_msg.range = distance;
  distance_msg.min_range = 0.0;
  distance_msg.max_range = 2.0;
  rcl_publish(&distance_publisher, &distance_msg, NULL);
  
  //接收订阅的数据，执行回调函数
   RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

在这个ESP32代码中，我们定义了一个measure_distance函数来触发超声波传感器并计算距离。然后，我们创建了一个发布者来发布距离数据，并设置了执行器来处理回调。
在setup函数中，我们初始化了超声波传感器的GPIO引脚，创建了一个发布者来发布距离数据，并设置了执行器来处理回调。
在loop函数中，我们调用rclc_executor_spin_some来处理回调。