ros小车搭建

一、硬件准备

上位机：树莓派4B*1（TF卡16G以上、读卡器）

下位机：arduino 2560*1

L298N电机驱动板*1

霍尔编码器减速电机*2

A1M8激光雷达

12V转5V降压模块

12V电源*1

充电宝*1

万向轮*1

面包板*1

杜邦线诺干

亚克力板*2

二、设计思路

双轮差速ros小车，通过下位机来控制小车运动，上位机输入指令，通过ros_arduino_bridge建立arduino和树莓派的消息通信。由于电机需要12V电压不能直接用arduino的供电，需要通过电机驱动板作为媒介，编码器直接连接到arduino。树莓派作为主机，PC作为从机来查看建图等功能的实现。

三、下位机程序

1.ros_arduino__bridge

该功能包包含Arduino库和用来控制Arduino的ROS驱动包，它旨在成为在ROS下运行Arduino控制的机器人的完整解决方案。

其中当前主要关注的是:功能包集中一个兼容不同驱动的机器人的基本控制器（base controller），它可以接收ROS Twist类型的消息，可以发布里程计数据。

下载:

进入ROS工作空间的src目录,输入命令:

git clone https://github.com/hbrobotics/ros_arduino_bridge.git

文件说明

├── ros_arduino_bridge                      # metapackage (元功能包)
│   ├── CMakeLists.txt
│   └── package.xml
├── ros_arduino_firmware                    #固件包，更新到Arduino
│   ├── CMakeLists.txt
│   ├── package.xml
│   └── src
│       └── libraries                       #库目录
│           ├── MegaRobogaiaPololu          #针对Pololu电机控制器，MegaRobogaia编码器的头文件定义
│           │   ├── commands.h              #定义命令头文件
│           │   ├── diff_controller.h       #差分轮PID控制头文件
│           │   ├── MegaRobogaiaPololu.ino  #PID实现文件
│           │   ├── sensors.h               #传感器相关实现，超声波测距，Ping函数
│           │   └── servos.h                #伺服器头文件
│           └── ROSArduinoBridge            #Arduino相关库定义
│               ├── commands.h              #定义命令
│               ├── diff_controller.h       #差分轮PID控制头文件
│               ├── encoder_driver.h        #编码器驱动头文件
│               ├── encoder_driver.ino      #编码器驱动实现, 读取编码器数据，重置编码器等
│               ├── motor_driver.h          #电机驱动头文件
│               ├── motor_driver.ino        #电机驱动实现，初始化控制器，设置速度
│               ├── ROSArduinoBridge.ino    #核心功能实现，程序入口
│               ├── sensors.h               #传感器头文件及实现
│               ├── servos.h                #伺服器头文件，定义插脚，类
│               └── servos.ino              #伺服器实现
├── ros_arduino_msgs                        #消息定义包
│   ├── CMakeLists.txt
│   ├── msg                                 #定义消息
│   │   ├── AnalogFloat.msg                 #定义模拟IO浮点消息
│   │   ├── Analog.msg                      #定义模拟IO数字消息
│   │   ├── ArduinoConstants.msg            #定义常量消息
│   │   ├── Digital.msg                     #定义数字IO消息
│   │   └── SensorState.msg                 #定义传感器状态消息
│   ├── package.xml
│   └── srv                                 #定义服务
│       ├── AnalogRead.srv                  #模拟IO输入
│       ├── AnalogWrite.srv                 #模拟IO输出
│       ├── DigitalRead.srv                 #数字IO输入
│       ├── DigitalSetDirection.srv　　　　 #数字IO设置方向
│       ├── DigitalWrite.srv                #数字IO输入
│       ├── ServoRead.srv                   #伺服电机输入
│       └── ServoWrite.srv                  #伺服电机输出
└── ros_arduino_python                      #ROS相关的Python包，用于上位机，树莓派等开发板或电脑等。
    ├── CMakeLists.txt
    ├── config                              #配置目录
    │   └── arduino_params.yaml             #定义相关参数，端口，rate，PID，sensors等默认参数。由arduino.launch调用
    ├── launch
    │   └── arduino.launch                  #启动文件
    ├── nodes
    │   └── arduino_node.py                 #python文件,实际处理节点，由arduino.launch调用，即可单独调用。
    ├── package.xml
    ├── setup.py
    └── src                                 #Python类包目录
        └── ros_arduino_python
            ├── arduino_driver.py           #Arduino驱动类
            ├── arduino_sensors.py          #Arduino传感器类
            ├── base_controller.py          #基本控制类，订阅cmd_vel话题，发布odom话题
            └── __init__.py                 #类包默认空文件

• ros_arduino_bridge/ros_arduino_firmware/src/libraries/ROSArduinoBridge
• ros_arduino_bridge/ros_arduino_python/config/arduino_params.yaml

前者是Arduino端的固件包实现，需要修改并上传至Arduino电路板；

后者是ROS端的一个配置文件，相关驱动已经封装完毕，我们只需要修改配置信息即可。

2.添加编码器驱动

在主文件中自定义的编码器驱动和电机驱动

在encoder_driver的头文件添加引脚定义以及需要用到的函数

#define ARDUINO_MY_COUNTER
  //定义引脚
  #define LEFT_A 18 //5
  #define LEFT_B 19 //4
  #define RIGHT_A 21 //2
  #define RIGHT_B 20 //3
  //声明函数
  //1.初始化函数：设置引脚操作模式，添加中断
  void initEncoder();
  //2.中断函数
  void leftEncoderEvent();
  void rightEncoderEvent();
#endif
//------------------------------------------------------------------
   
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();

encoder_driver文件内容

#ifdef USE_BASE
#ifdef defined ARDUINO_MY_COUNTER
  //功能左右电机的脉冲计数
  //计数器
  volatile long left_count = 0L;
  volatile long right_count = 0L;
  //初始化
  void initEncoder(){
    pinMode(LEFT_A,INPUT);
    pinMode(LEFT_B,INPUT);
    pinMode(RIGHT_A,INPUT);
    pinMode(RIGHT_B,INPUT);

    //添加中断
    attachInterrupt(5,leftEncoderEvent,CHANGE);
    //attachInterrupt(4,leftEncoderEvent,CHANGE);
    attachInterrupt(2,rightEncoderEvent,CHANGE);
    //attachInterrupt(1,rightEncoderEvent,CHANGE);
  }
  //编写中断的回调函数
   void leftEncoderEvent(){
    if(digitalRead(LEFT_A) == HIGH){
      if(digitalRead(LEFT_B) == HIGH){
        left_count++;
      } else {
        left_count--;
      }
    } else {
      if(digitalRead(LEFT_B) == LOW){
        left_count++;
      } else {
        left_count--;
      }
    }
   }
   void rightEncoderEvent(){
    if(digitalRead(RIGHT_A) == HIGH){
      if(digitalRead(RIGHT_B) == HIGH){
        right_count++;
      } else {
        right_count--;
      }
    } else {
      if(digitalRead(RIGHT_B) == LOW){
        right_count++;
      } else {
        right_count--;
      }
    }  
   }
  //实现编码器数据读和重置 
  long readEncoder(int i){
    if (i == LEFT)
      return left_count;
    if (i == RIGHT)
      return right_count;
  }
  void resetEncoder(int i){
    if (i == LEFT){
      left_count = 0L;
      return;
    } else { 
      right_count = 0L;
      return;
    }
  }
#endif

/* Wrap the encoder reset function */
void resetEncoders() {
  resetEncoder(LEFT);
  resetEncoder(RIGHT);
}

#endif

使用两倍频计数

3.添加电机驱动模块

在motor_driver的头文件添加引脚定义以及需要用到的函数

#define L298N_MOTOR_DRIVER
  #define LEFT_DIR1 7
  #define LEFT_DIR2 8
  #define LEFT_PWM 5
  #define RIGHT_DIR1 9
  #define RIGHT_DIR2 10
  #define RIGHT_PWM 6
#endif

void initMotorController();
void setMotorSpeed(int i, int spd);
void setMotorSpeeds(int leftSpeed, int rightSpeed);

motor_driver内容

#ifdef USE_BASE
#ifdef defined L298N_MOTOR_DRIVER
  //初始化
  void initMotorController(){
    pinMode(LEFT_DIR1,OUTPUT);
    pinMode(LEFT_DIR2,OUTPUT);
    pinMode(LEFT_PWM,OUTPUT);
    pinMode(RIGHT_DIR1,OUTPUT);
    pinMode(RIGHT_DIR2,OUTPUT);
    pinMode(RIGHT_PWM,OUTPUT);
  }
  //设置单个电机的速度
  void setMotorSpeed(int i, int spd){
    unsigned char reverse = 0;
    if (spd < 0)
    {
      spd = -spd;
      reverse = 1;
    }
    if (spd > 255)
      spd = 255;
    
    if (i == LEFT) { 
      if(reverse == 1) { //正转
        digitalWrite(LEFT_DIR1,HIGH);
        digitalWrite(LEFT_DIR2,LOW);
      }
      else if (reverse == 0) { 
        digitalWrite(LEFT_DIR1,LOW);
        digitalWrite(LEFT_DIR2,HIGH);
      }
       analogWrite(LEFT_PWM,spd);
    }
    if (i == RIGHT) { 
      if(reverse == 1) { //正转
        digitalWrite(RIGHT_DIR1,HIGH);
        digitalWrite(RIGHT_DIR2,LOW);
      }
      else if (reverse == 0) { 
        digitalWrite(RIGHT_DIR1,LOW);
        digitalWrite(RIGHT_DIR2,HIGH);
      }
      analogWrite(RIGHT_PWM,spd);
    }
  }
  //设置两个电机的速度
  void setMotorSpeeds(int leftSpeed, int rightSpeed){
    setMotorSpeed(RIGHT, rightSpeed);
    setMotorSpeed(LEFT, leftSpeed);
  }
#endif

#endif

4.实现PID调试

之前调试上面kp kd ki三个值直到稳定

5.添加舵机驱动

Arduino servo库函数_

servos.h文件

#ifndef SERVOS_H
#define SERVOS_H


#define N_SERVOS 4 //定义四轴机械臂
int stepDelay [N_SERVOS] = { 0, 0 }; // ms
// Pins
byte servoPins [N_SERVOS] = { 11 , 12 , 44 ,45 }; //定义引脚
// 初始位置
byte servoInitPosition [N_SERVOS] = { 90, 90 , 90 , 90 }; // [0, 180] degrees

class SweepServo //定义舵机类
{
  public:
    SweepServo();
    void initServo(
        int servoPin, //引脚
        int stepDelayMs, // ms
        int initPosition); //初始位置
    void doSweep(); 
    void setTargetPosition(int position);
    Servo getServo();

  private:
    Servo servo;
    int stepDelayMs;
    int currentPositionDegrees;
    int targetPositionDegrees;
    long lastSweepCommand;
};
SweepServo servos [N_SERVOS]; //创建对象
#endif

servos文件

#ifdef USE_SERVOS
// 构造函数
SweepServo::SweepServo()
{
  this->currentPositionDegrees = 0;
  this->targetPositionDegrees = 0;
  this->lastSweepCommand = 0;
}
// 舵机初始化
void SweepServo::initServo(
    int servoPin,
    int stepDelayMs,
    int initPosition)
{
  this->servo.attach(servoPin);
  this->stepDelayMs = stepDelayMs;
  this->currentPositionDegrees = initPosition;
  this->targetPositionDegrees = initPosition; 
  this->lastSweepCommand = millis();
}

//舵机的扫描运动
void SweepServo::doSweep()
{

  //获取当前时间和上一次扫描命令的时间差
  int delta = millis() - this->lastSweepCommand;

  // 检查是否到达下一个步进的时间
  if (delta > this->stepDelayMs) {
    // 检查步进的方向
    if (this->targetPositionDegrees > this->currentPositionDegrees) {
      // 如果目标位置大于当前位置，就增加当前位置的角度
      this->currentPositionDegrees++;
      // 把当前位置的角度写入舵机
      this->servo.write(this->currentPositionDegrees);
    }
    else if (this->targetPositionDegrees < this->currentPositionDegrees) {
      // 如果目标位置小于当前位置，就减少当前位置的角度
      this->currentPositionDegrees--;
      // 把当前位置的角度写入舵机
      this->servo.write(this->currentPositionDegrees);
    }
    // 如果目标位置等于当前位置，就不做任何操作

    // 重置计时器
    this->lastSweepCommand = millis();
  }
}


// 设置偏转角度
void SweepServo::setTargetPosition(int position)
{
  this->targetPositionDegrees = position;
}


// 操作舵机目标
Servo SweepServo::getServo()
{
  return this->servo;
}

#endif

四、上位机程序

1.树莓派4B安装ubuntu18+ROS环境

最新版的4B是不支持自定义镜像的安装，需要再安装驱动放到镜像文件中，准备一个16G以上的内存卡和读卡器，使用Raspberry Pi Imager官方软件写入镜像文件

将下载好的固件文件直接放在烧录好系统的内存卡中，全部覆盖文件

如果要安装别的版本，2022.4.4 版本更新的时候将默认用户 pi 删除，取消了默认的账户密码，也就是原来一直使用 的 pi 和对应的默认密码 raspberry 被取消了，如果用旧的方法写入最新系统，最后远程 SSH 也会报 Access Denied 报错。

而且自定的镜像的话需要安装上面的固件

官方烧录工具

2.分布式框架搭建

1.树莓派设置静态IP

2.分别修改不同计算机的 /etc/hosts 文件，在该文件中加入对方的IP地址和计算机名:

主机端: 从机的IP 从机计算机名

从机端: 主机的IP 主机计算机名

3.配置~/.bashrc

树莓派端：

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=主机IP

PC端：

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=从机IP

测试

启动小乌龟案例，roscore必须在树莓派开

(补充：.bashrc是在用户目录的下的隐藏文件使用cd home/你用用户名 && vi .bashrc打开)

3.ssh远程连接

1.安装SSH客户端与服务端

sudo apt- install openssh-server

sudo apt-get install openssh-client

2.服务端启动SSH服务

树莓派启动 ssh 服务:

sudo /etc/init.d/ssh start

设置开机自启

sudo systemctl enable ssh

3.客户端远程登陆服务端

ssh 账号@ip地址

然后根据提示，录入登陆密码，即可成功登陆。

4.实现数据传输

上传文件:

scp 本地文件路径 账号@ip:树莓派路径

上传文件夹:

scp -r 本地文件夹路径 账号@ip:树莓派路径

之后树莓派就不需要显示器了，接通电源之后直接使用PC机远程连接

4.控制实现

以下都是远程连接树莓派，在树莓派的操作

先将之前的ros_arduino_bridge上传到树莓派

安装功能包

sudo apt install python-serial

修改文件

cd /home/你的用户名/ros_arduino_bridge-indigo-devel/ros_arduino_python/config
cp arduino_params.yaml my_arduino_params.yaml
vi my_arduino_params.yaml

文件内容

port: /dev/ttyUSB0 #视情况设置，一般设置为 /dev/ttyACM0 或 /dev/ttyUSB0
baud: 57600 #波特率
timeout: 0.1 #超时时间

rate: 50
sensorstate_rate: 10

use_base_controller: True  #启用基座控制器
base_controller_rate: 10   

base_frame: base_footprint #base_frame 设置

# === Robot drivetrain parameters
wheel_diameter: 0.065 #车轮直径
wheel_track: 0.21 #轮间距
encoder_resolution: 3960#编码器精度(一圈的脉冲数 * 倍频 * 减速比)
#gear_reduction: 1 #减速比
#motors_reversed: False #转向取反

# === PID parameters PID参数，根据3.4的PID调试结果填写
Kp: 5
Kd: 45
Ki: 0
Ko: 50
accel_limit: 1.0

# === Sensor definitions.  Examples only - edit for your robot.
#     Sensor type can be one of the follow (case sensitive!):
#      * Ping
#      * GP2D12
#      * Analog
#      * Digital
#      * PololuMotorCurrent
#      * PhidgetsVoltage
#      * PhidgetsCurrent (20 Amp, DC)



sensors: {
  #motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
  #motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
  #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
  #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
  arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output} //闪烁LED
}

5.源码解析

launch文件

<launch>
   <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
      <!--加载一个参数文件到参数服务器 -->
      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
   </node>
</launch>

nodes文件

#!/usr/bin/env python
import rospy
from ros_arduino_python.arduino_driver import Arduino  # 导入Arduino驱动类
from ros_arduino_python.arduino_sensors import *       # 导入Arduino驱动类
from ros_arduino_msgs.srv import *                     # 导入Arduino相关的服务消息类型
from ros_arduino_python.base_controller import BaseController #导入基础控制类
from geometry_msgs.msg import Twist                    # 导入Twist消息类型，用于表示速度和角速度
import os, time  #系统时间操作功能包
import thread    #多线程
from serial.serialutil import SerialException          # 导入SerialException异常类，用于处理串口通信错误

class ArduinoROS():           # 定义一个类，表示Arduino与ROS的交互
    def __init__(self):
        rospy.init_node('arduino', log_level=rospy.INFO)   # 初始化一个节点，命名为arduino，并设置日志等级为INFO
        self.name = rospy.get_name()  # 获取节点的名称，并赋值给self.name属性
        rospy.on_shutdown(self.shutdown)  # 注册一个回调函数，在节点关闭时执行
        self.port = rospy.get_param("~port", "/dev/ttyUSB0") # 获取串口参数，默认为/dev/ttyACM0，并赋值给self.port属性
        self.baud = int(rospy.get_param("~baud", 57600))   # 获取波特率参数，默认为57600，并赋值给self.baud属性
        self.timeout = rospy.get_param("~timeout", 0.5) # 获取超时参数，默认为0.5秒，并赋值给self.timeout属性
        self.base_frame = rospy.get_param("~base_frame", 'base_link') # 获取基础坐标系参数
        self.motors_reversed = rospy.get_param("~motors_reversed", False)# 获取电机反转参数，默认为False，并赋值给self.motors_reversed属性

        self.rate = int(rospy.get_param("~rate", 50)) # 获取节点运行频率参数，默认为50Hz，并赋值给self.rate属性
        r = rospy.Rate(self.rate)  # 创建一个Rate对象，用于控制节点运行速度

        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))  # 获取传感器状态发布频率参数，默认为10Hz，并赋值给self.sensorstate_rate属性
        
        self.use_base_controller = rospy.get_param("~use_base_controller", False)# 获取是否使用基础控制器参数，默认为False，并赋值给self.use_base_controller属性
        
        now = rospy.Time.now()# 获取当前时间，并赋值给now变量
        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)  # 计算传感器状态发布间隔时间，并赋值给self.t_delta_sensors属性
        self.t_next_sensors = now + self.t_delta_sensors # 计算下一次传感器状态发布时间，并赋值给self.t_next_sensors属性
        
        self.cmd_vel = Twist()# 创建一个Twist对象，用于存储速度和角速度命令，并赋值给self.cmd_vel属性

        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)# 创建一个发布者对象，用于发布速度和角速度命令到cmd_vel话题，并设置队列大小为5，并赋值给self.cmd_vel_pub属性

        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)  # 创建一个发布者对象，用于发布传感器状态到~sensor_state话题，并设置队列大小为5，并赋值给self.sensorStatePub属性

        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)# 创建一个服务对象，用于提供~servo_write服务，接收ServoWrite类型的请求，并调用self.ServoWriteHandler方法处理

        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) # 创建一个服务对象，用于提供~servo_read服务，接收ServoRead类型的请求，并调用self.ServoReadHandler方法处理

        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) # 创建一个服务对象，用于提供~digital_set_direction服务，接收DigitalSetDirection类型的请求，并调用self.DigitalSetDirectionHandler方法处理

        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler) # 创建一个服务对象，用于提供~digital_write服务，接收DigitalWrite类型的请求，并调用self.DigitalWriteHandler方法处理

        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)# 创建一个服务对象，用于提供~digital_read服务，接收DigitalRead类型的请求，并调用self.DigitalReadHandler方法处理

        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler) # 创建一个服务对象，用于提供~analog_write服务，接收AnalogWrite类型的请求，并调用self.AnalogWriteHandler方法处理

        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)# 创建一个服务对象，用于提供~analog_read服务，接收AnalogRead类型的请求，并调用self.AnalogReadHandler方法处理

        self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed) # 创建一个Arduino对象，用于与Arduino通信，并赋值给self.controller属性

        self.controller.connect()# 调用Arduino对象的connect方法，连接Arduino设备

        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
 # 打印日志信息，显示已经连接到Arduino设备
     
        mutex = thread.allocate_lock()  # 创建一个互斥锁对象，用于同步多线程操作，并赋值给mutex变量

        self.mySensors = list()# 创建一个空列表对象，用于存储传感器对象，并赋值给self.mySensors属性

        sensor_params = rospy.get_param("~sensors", dict({}))# 获取传感器参数，默认为空字典，并赋值给sensor_params变量

        for name, params in sensor_params.iteritems():# 遍历传感器参数中的每一项
       #这里并没有使用到传感器
            try:
                params['direction']
            except:
                params['direction'] = 'input'

            if params['type'] == "Ping":
                sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == "GP2D12":
                sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'Digital':
                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'Analog':
                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
            elif params['type'] == 'PololuMotorCurrent':
                sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsVoltage':
                sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
            elif params['type'] == 'PhidgetsCurrent':
                sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
       #-----------------------------------------------------------------------------         
            try:
                self.mySensors.append(sensor)
                rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
            except:
                rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")

			if self.use_base_controller:  # 如果self.use_base_controller属性为真
            	self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller")  # 创建一个BaseController对象，用于控制机器人的运动，并赋值给self.myBaseController属性

        while not rospy.is_shutdown():  # 当节点没有关闭时，循环执行以下代码
            for sensor in self.mySensors:  # 遍历self.mySensors列表中的每一个传感器对象
                mutex.acquire()  # 获取互斥锁，防止其他线程干扰
                sensor.poll()  # 调用传感器对象的poll方法，读取传感器数据并发布到相应话题
                mutex.release()  # 释放互斥锁，允许其他线程操作

            if self.use_base_controller:  # 如果self.use_base_controller属性为真
                mutex.acquire()  # 获取互斥锁，防止其他线程干扰
                self.myBaseController.poll()  # 调用基础控制器对象的poll方法，读取速度和角速度命令并发送给Arduino
                mutex.release()  # 释放互斥锁，允许其他线程操作


            now = rospy.Time.now()  # 获取当前时间，并赋值给now变量

            if now > self.t_next_sensors:  # 如果当前时间大于下一次传感器状态发布时间
                msg = SensorState()  # 创建一个SensorState对象，用于存储所有传感器的名称和值，并赋值给msg变量
                msg.header.frame_id = self.base_frame  # 设置消息头中的坐标系为self.base_frame属性
                msg.header.stamp = now  # 设置消息头中的时间戳为当前时间
                for i in range(len(self.mySensors)):  # 遍历self.mySensors列表中的每一个传感器对象的索引
                    msg.name.append(self.mySensors[i].name)  # 把传感器对象的名称添加到msg.name列表中
                    msg.value.append(self.mySensors[i].value)  # 把传感器对象的值添加到msg.value列表中
                try:  # 尝试执行以下代码
                    self.sensorStatePub.publish(msg)  # 调用self.sensorStatePub发布者对象的publish方法，发布msg消息到~sensor_state话题
                except:  # 如果发生异常
                    pass  # 不做任何处理

                self.t_next_sensors = now + self.t_delta_sensors  # 更新下一次传感器状态发布时间为当前时间加上传感器状态发布间隔时间

            r.sleep()  # 调用Rate对象的sleep方法，让节点按照设定的频率运行
#----------------------------------------------------------------------------------------------
    def ServoWriteHandler(self, req):  # 定义一个方法，用于处理~servo_write服务的请求，参数为req
        self.controller.servo_write(req.id, req.value)  # 调用Arduino对象的servo_write方法，向指定id的舵机写入指定值
        return ServoWriteResponse()  # 返回一个ServoWriteResponse对象，表示服务响应

    def ServoReadHandler(self, req):  # 定义一个方法，用于处理~servo_read服务的请求，参数为req
        pos = self.controller.servo_read(req.id)  # 调用Arduino对象的servo_read方法，从指定id的舵机读取位置，并赋值给pos变量
        return ServoReadResponse(pos)  # 返回一个ServoReadResponse对象，表示服务响应，包含pos变量

    def DigitalSetDirectionHandler(self, req):  # 定义一个方法，用于处理~digital_set_direction服务的请求，参数为req
        self.controller.pin_mode(req.pin, req.direction)  # 调用Arduino对象的pin_mode方法，设置指定引脚的方向
        return DigitalSetDirectionResponse()  # 返回一个DigitalSetDirectionResponse对象，表示服务响应

    def DigitalWriteHandler(self, req):  # 定义一个方法，用于处理~digital_write服务的请求，参数为req
        self.controller.digital_write(req.pin, req.value)  # 调用Arduino对象的digital_write方法，向指定引脚写入数字值
        return DigitalWriteResponse()  # 返回一个DigitalWriteResponse对象，表示服务响应

    def DigitalReadHandler(self, req):  # 定义一个方法，用于处理~digital_read服务的请求，参数为req
        value = self.controller.digital_read(req.pin)  # 调用Arduino对象的digital_read方法，从指定引脚读取数字值，并赋值给value变量
        return DigitalReadResponse(value)  # 返回一个DigitalReadResponse对象，表示服务响应，包含value变量

    def AnalogWriteHandler(self, req):  # 定义一个方法，用于处理~analog_write服务的请求，参数为req
        self.controller.analog_write(req.pin, req.value)  # 调用Arduino对象的analog_write方法，向指定引脚写入模拟值
        return AnalogWriteResponse()  # 返回一个AnalogWriteResponse对象，表示服务响应

    def AnalogReadHandler(self, req):  # 定义一个方法，用于处理~analog_read服务的请求，参数为req
        value = self.controller.analog_read(req.pin)  # 调用Arduino对象的analog_read方法，从指定引脚读取模拟值，并赋值给value变量
        return AnalogReadResponse(value)  # 返回一个AnalogReadResponse对象，表示服务响应，包含value变量

    def shutdown(self):  # 定义一个方法，在节点关闭时执行
        rospy.loginfo("Shutting down Arduino Node...")  # 打印日志信息，显示正在关闭Arduino节点

     
        try:  # 尝试执行以下代码
            rospy.loginfo("Stopping the robot...")  # 打印日志信息，显示正在停止机器人
            self.cmd_vel_pub.Publish(Twist())  # 调用self.cmd_vel_pub发布者对象的publish方法，发布一个空的Twist消息到cmd_vel话题，让机器人停止运动
            rospy.sleep(2)  # 等待2秒
        except:  # 如果发生异常
            pass  # 不做任何处理

     
        try:  # 尝试执行以下代码
            self.controller.close()  # 调用Arduino对象的close方法，关闭与Arduino设备的连接
        except:  # 如果发生异常
            pass  # 不做任何处理
        finally:  # 最后执行以下代码
            rospy.loginfo("Serial port closed.")  # 打印日志信息，显示串口已经关闭
            os._exit(0)  # 强制退出程序

if __name__ == '__main__':  # 如果这个文件是主程序
    try:  # 尝试执行以下代码
        myArduino = ArduinoROS()  # 创建一个ArduinoROS对象，并赋值给myArduino变量
    except SerialException:  # 如果发生SerialException异常
        rospy.logerr("Serial exception trying to open port.")  # 打印错误信息，显示尝试打开串口时发生异常
        os._exit(0)  # 强制退出程序

src文件

arduino_driver.py

#!/usr/bin/env python
import thread
from math import pi as PI, degrees, radians
import os
import time
import sys, traceback
from serial.serialutil import SerialException
from serial import Serial
 
SERVO_MAX = 180 #舵机最大角度
SERVO_MIN = 0   #舵机最小角度

class Arduino:  

    N_ANALOG_PORTS = 6  #模拟端口数量
    N_DIGITAL_PORTS = 12  #数字端口的数量

    def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False):

        self.PID_RATE = 30 #固定值，不可改变
        self.PID_INTERVAL = 1000 / 30  #PID控制器间隔时间

        self.port = port
        self.baudrate = baudrate 
        self.timeout = timeout 
        self.encoder_count = 0 #编码计数
        self.writeTimeout = timeout
        self.interCharTimeout = timeout / 30.
        self.motors_reversed = motors_reversed #电机是否反转

        self.mutex = thread.allocate_lock()

        self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS

        self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS

    def connect(self): #定义函数连接Arduino
        try:
            print "Connecting to Arduino on port", self.port, "..."
            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)

            time.sleep(1)
            test = self.get_baud()  #再次调用self.get_baud方法，获取Arduino设备的波特率，并赋值给test变量
            if test != self.baudrate: 
                time.sleep(1) 
                test = self.get_baud()
                if test != self.baudrate: # 如果test变量仍然不等于self.baudrate属性
                    raise SerialException #异常退出
            print "Connected at", self.baudrate
            print "Arduino is ready."

        except SerialException:
            print "Serial Exception:"
            print sys.exc_info()
            print "Traceback follows:"
            traceback.print_exc(file=sys.stdout)
            print "Cannot connect to Arduino!"
            os._exit(1)

    def open(self): #打开串口

        self.port.open()

    def close(self): #关闭串口

        self.port.close()
	
    def send(self, cmd):  # 定义一个实例方法，用于发送命令到Arduino设备，参数为cmd

        self.port.write(cmd + '\r')  # 调用Serial对象的write方法，写入cmd参数加上回车符到串口

    def recv(self, timeout=0.5): #定义一个实例方法，用于接收Arduino设备的响应，参数为timeout，默认为0.5秒
        timeout = min(timeout, self.timeout)

        c = ''
        value = ''
        attempts = 0 #记录字符大小
        while c != '\r':  # 当c变量不等于回车符时
            c = self.port.read(1) # 调用Serial对象的read方法，从串口读取一个字节，并赋值给c变量
            value += c  # 把c变量的值追加到value变量中
            attempts += 1  # 把attempts变量加1
            if attempts * self.interCharTimeout > timeout:  # 如果attempts变量乘以self.interCharTimeout属性大于timeout变量
                return None

        value = value.strip('\r') # 把value变量中的回车符去掉，并赋值给value变量

        return value # 返回value变量

    def recv_ack(self):  # 定义一个实例方法，用于接收Arduino设备的确认信号

        ack = self.recv(self.timeout) # 调用self.recv方法，接收Arduino设备的响应，并赋值给ack变量
        return ack == 'OK'

    def recv_int(self): # 定义一个实例方法，用于接收Arduino设备的整数响应

        value = self.recv(self.timeout)
        try:
            return int(value)
        except:
            return None

    def recv_array(self):

        try:
            values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
            return map(int, values)
        except:
            return []

    def execute(self, cmd):  # 定义一个实例方法，用于执行命令并返回整数响应，参数为cmd

        self.mutex.acquire() # 调用互斥锁对象的acquire方法，获取锁

        try:
            self.port.flushInput() # 调用Serial对象的flushInput方法，清空输入缓冲区
        except:
            pass

        ntries = 1
        attempts = 0

        try:
            self.port.write(cmd + '\r') # 调用Serial对象的write方法，写入cmd参数加上回车符到串口
            value = self.recv(self.timeout)# 调用self.recv方法，接收Arduino设备的响应，并赋值给value变量
            while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
                 # 当已经尝试次数小于尝试次数且value变量为空字符串或者'Invalid Command'或者None时，循环执行以下代码
                try:
                    self.port.flushInput()
                    self.port.write(cmd + '\r')
                    value = self.recv(self.timeout)
                except:
                    print "Exception executing command: " + cmd
                attempts += 1
        except:
            self.mutex.release()
            print "Exception executing command: " + cmd
            value = None

        self.mutex.release()
        return int(value)

    def execute_array(self, cmd):

        self.mutex.acquire()

        try:
            self.port.flushInput()
        except:
            pass

        ntries = 1
        attempts = 0

        try:
            self.port.write(cmd + '\r')
            values = self.recv_array()
            while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
                try:
                    self.port.flushInput()
                    self.port.write(cmd + '\r')
                    values = self.recv_array()
                except:
                    print("Exception executing command: " + cmd)
                attempts += 1
        except:
            self.mutex.release()
            print "Exception executing command: " + cmd
            raise SerialException
            return []

        try:
            values = map(int, values)
        except:
            values = []

        self.mutex.release()
        return values

    def execute_ack(self, cmd):# 定义一个实例方法，用于执行命令并返回确认信号，参数为cmd

        self.mutex.acquire()

        try:
            self.port.flushInput()
        except:
            pass

        ntries = 1
        attempts = 0

        try:
            self.port.write(cmd + '\r')
            ack = self.recv(self.timeout)
            while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
                try:
                    self.port.flushInput()
                    self.port.write(cmd + '\r')
                    ack = self.recv(self.timeout)
                except:
                    print "Exception executing command: " + cmd
            attempts += 1
        except:
            self.mutex.release()
            print "execute_ack exception when executing", cmd
            print sys.exc_info()
            return 0

        self.mutex.release()
        return ack == 'OK'

    def update_pid(self, Kp, Kd, Ki, Ko):# 定义一个实例方法，用于更新PID控制器的参数，参数为Kp, Kd, Ki, Ko

        print "Updating PID parameters"
        cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
        self.execute_ack(cmd)

    def get_baud(self): # 定义一个实例方法，用于获取Arduino设备的波特率

        try:
            return int(self.execute('b'));
        except:
            return None

    def get_encoder_counts(self): # 定义一个实例方法，用于获取编码器的计数
        values = self.execute_array('e')
        if len(values) != 2:
            print "Encoder count was not 2"
            raise SerialException
            return None
        else:
            if self.motors_reversed:
                values[0], values[1] = -values[0], -values[1]
            return values

    def reset_encoders(self): # 定义一个实例方法，用于重置编码器

        return self.execute_ack('r')
#---------------------------------------------------------------------------------------
    def drive(self, right, left):# 定义一个实例方法，用于驱动电机，参数为right和left

        if self.motors_reversed:
            left, right = -left, -right
        return self.execute_ack('m %d %d' %(right, left)) # 返回调用self.execute_ack方法，执行'm %d %d'命令并返回确认信号的值，其中%d分别用right和left变量的值替换
    def drive_m_per_s(self, right, left):# 定义一个实例方法，用于以米/秒为单位驱动电机，参数为right和left

        left_revs_per_second = float(left) / (self.wheel_diameter * PI)
        right_revs_per_second = float(right) / (self.wheel_diameter * PI)

        left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
        right_ticks_per_loop  = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)

        self.drive(right_ticks_per_loop , left_ticks_per_loop )

    def stop(self): # 定义一个实例方法，用于停止电机

        self.drive(0, 0) # 调用self.drive方法，向电机发送0值

    def analog_read(self, pin):# 定义一个实例方法，用于从模拟端口读取值，参数为pin
        return self.execute('a %d' %pin)

    def analog_write(self, pin, value):# 定义一个实例方法，用于向模拟端口写入值，参数为pin和value
        return self.execute_ack('x %d %d' %(pin, value))

    def digital_read(self, pin):  # 定义一个实例方法，用于从数字端口读取值，参数为pin
        return self.execute('d %d' %pin)

    def digital_write(self, pin, value):# 定义一个实例方法，用于向数字端口写入值，参数为pin和value
        return self.execute_ack('w %d %d' %(pin, value))

    def pin_mode(self, pin, mode): #定义一个实例方法，用于设置引脚的模式，参数为pin和mode
        return self.execute_ack('c %d %d' %(pin, mode))

    def servo_write(self, id, pos):# 定义一个实例方法，用于向舵机写入位置，参数为id和pos

        return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))

    def servo_read(self, id):# 定义一个实例方法，用于从舵机读取位置，参数为id
        return radians(self.execute('t %d' %id))

    def ping(self, pin):
        return self.execute('p %d' %pin);
if __name__ == "__main__":
    if os.name == "posix":
        portName = "/dev/ttyACM0"
    else:
        portName = "COM43" # Windows style COM port.

    baudRate = 57600

    myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
    myArduino.connect()

    print "Sleeping for 1 second..."
    time.sleep(1)

    print "Reading on analog port 0", myArduino.analog_read(0)
    print "Reading on digital port 0", myArduino.digital_read(0)
    print "Blinking the LED 3 times"
    for i in range(3):
        myArduino.digital_write(13, 1)
        time.sleep(1.0)
    print "Connection test successful.",

    myArduino.stop()
    myArduino.close()

    print "Shutting down Arduino."

如果修改了commands.h文件的内容，需要对应修改操作字符

base_controller.py

#!/usr/bin/env python
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
import os

from math import sin, cos, pi
from geometry_msgs.msg import Quaternion, Twist, Pose# 导入geometry_msgs包中的Quaternion, Twist, Pose消息类型，用于表示姿态、速度和位置
from nav_msgs.msg import Odometry# 导入nav_msgs包中的Odometry消息类型，用于表示里程计数据
from tf.broadcaster import TransformBroadcaster# 导入tf包中的TransformBroadcaster类，用于发布坐标变换

class BaseController: # 定义一个类，表示基础控制器，用于接收Twist命令和发布Odometry数据
    def __init__(self, arduino, base_frame, name="base_controllers"):
        self.arduino = arduino # 定义一个实例属性
        self.name = name
        self.base_frame = base_frame
        self.rate = float(rospy.get_param("~base_controller_rate", 10))# 定义一个实例属性，表示发布频率，并赋值为从参数服务器获取~base_controller_rate参数的值转换为浮点数，默认为10
        self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
        self.stopped = False
                 
        pid_params = dict()# 定义一个变量pid_params，并赋值为空字典，用于存储PID控制器的参数
        pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 
        pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 
        pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
        pid_params['Kp'] = rospy.get_param("~Kp", 20)
        pid_params['Kd'] = rospy.get_param("~Kd", 12)
        pid_params['Ki'] = rospy.get_param("~Ki", 0)
        pid_params['Ko'] = rospy.get_param("~Ko", 50)
        
        self.accel_limit = rospy.get_param('~accel_limit', 0.1)
        self.motors_reversed = rospy.get_param("~motors_reversed", False)
        
        # Set up PID parameters and check for missing values
        self.setup_pid(pid_params)
            
        # How many encoder ticks are there per meter?
        self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi) # 计算每米需要转动的编码器计数，并赋值给self.ticks_per_meter属性
        
        # What is the maximum acceleration we will tolerate when changing wheel speeds?
        self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
        #计算改变轮速时允许的最大加速度，并赋值给self.max_accel属性
        # Track how often we get a bad encoder count (if any)
        self.bad_encoder_count = 0
        # 定义一个实例属性，表示错误的编码器计数次数，并赋值为0             
        now = rospy.Time.now()    
        self.then = now # time for determining dx/dy
        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = now + self.t_delta

        # Internal data        
        self.enc_left = None            # encoder readings
        self.enc_right = None
        self.x = 0                      # position in xy plane
        self.y = 0
        self.th = 0                     # rotation in radians
        self.v_left = 0
        self.v_right = 0
        self.v_des_left = 0             # cmd_vel setpoint
        self.v_des_right = 0
        self.last_cmd_vel = now
#-------------------------------------------------------------------------------------
        # Subscriptions
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)# 创建一个订阅者对象，订阅cmd_vel话题，消息类型为Twist，回调函数为self.cmdVelCallback
        
        self.arduino.reset_encoders()

        self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5) # 创建一个发布者对象，发布odom话题，消息类型为Odometry，队列大小为5，并赋值给self.odomPub属性
        self.odomBroadcaster = TransformBroadcaster() # 创建一个TransformBroadcaster对象，并赋值给self.odomBroadcaster属性
        
        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
        rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
        
    def setup_pid(self, pid_params): # 定义一个实例方法，用于设置PID控制器的参数，参数为pid_params
   
        missing_params = False
        for param in pid_params:
            if pid_params[param] == "":
                print("*** PID Parameter " + param + " is missing. ***")
                missing_params = True
        
        if missing_params:
            os._exit(1)
                
        self.wheel_diameter = pid_params['wheel_diameter']
        self.wheel_track = pid_params['wheel_track']
        self.encoder_resolution = pid_params['encoder_resolution']
        self.gear_reduction = pid_params['gear_reduction']
        
        self.Kp = pid_params['Kp']
        self.Kd = pid_params['Kd']
        self.Ki = pid_params['Ki']
        self.Ko = pid_params['Ko']
        
        self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)

    def poll(self): # 定义一个实例方法，用于轮询Arduino设备和发布里程计数据
        now = rospy.Time.now() # 获取当前时间
        if now > self.t_next: # 如果当前时间超过了下一次轮询的时间
            # Read the encoders # 读取编码器的值
            try:
                left_enc, right_enc = self.arduino.get_encoder_counts() # 尝试从Arduino获取左右编码器的计数
            except: # 如果出现异常
                self.bad_encoder_count += 1 # 增加错误计数
                rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) # 记录错误日志
                return # 返回，不进行后续操作
                            
            dt = now - self.then # 计算时间间隔
            self.then = now # 更新上一次轮询的时间
            dt = dt.to_sec() # 将时间间隔转换为秒
            
            # Calculate odometry # 计算里程计数据
            if self.enc_left == None: # 如果左编码器的值为空
                dright = 0 # 右侧移动距离为0
                dleft = 0 # 左侧移动距离为0
            else: # 否则
                dright = (right_enc - self.enc_right) / self.ticks_per_meter # 根据编码器计数和每米刻度数，计算右侧移动距离
                dleft = (left_enc - self.enc_left) / self.ticks_per_meter # 计算左侧移动距离

            self.enc_right = right_enc # 更新右编码器的值
            self.enc_left = left_enc # 更新左编码器的值
            
            dxy_ave = (dright + dleft) / 2.0 # 计算平均移动距离
            dth = (dright - dleft) / self.wheel_track # 根据两侧移动距离和轮距，计算转角变化量
            vxy = dxy_ave / dt # 计算线速度
            vth = dth / dt # 计算角速度
                
            if (dxy_ave != 0): # 如果有移动距离
                dx = cos(dth) * dxy_ave # 计算x方向的位移分量
                dy = -sin(dth) * dxy_ave # 计算y方向的位移分量
                self.x += (cos(self.th) * dx - sin(self.th) * dy) # 根据当前角度，更新x坐标
                self.y += (sin(self.th) * dx + cos(self.th) * dy) # 更新y坐标
    
            if (dth != 0): # 如果有转角变化量
                self.th += dth  # 更新当前角度
    
            quaternion = Quaternion() # 创建一个四元数对象，用于表示方向
            quaternion.x = 0.0  # 设置x分量为0 
            quaternion.y = 0.0  # 设置y分量为0
            quaternion.z = sin(self.th / 2.0) # 根据当前角度，计算z分量
            quaternion.w = cos(self.th / 2.0) # 计算w分量
    
            # Create the odometry transform frame broadcaster. 创建一个里程计变换帧广播器，用于发布里程计数据和机器人姿态变换信息。
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),  # 发送机器人的位置信息，z坐标为0 
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w), # 发送机器人的方向信息，用四元数表示
                rospy.Time.now(), # 发送当前时间戳
                self.base_frame, # 发送机器人的基准坐标系名称，即"base_link"
                "odom"  # 发送里程计坐标系名称，即"odom"
                )
    
            odom = Odometry() # 创建一个里程计对象，用于存储里程计数据
            odom.header.frame_id = "odom" # 设置里程计数据的参考坐标系为"odom"
            odom.child_frame_id = self.base_frame # 设置里程计数据的子坐标系为机器人的基准坐标系
            odom.header.stamp = now # 设置里程计数据的时间戳为当前时间
            odom.pose.pose.position.x = self.x # 设置机器人的x坐标
            odom.pose.pose.position.y = self.y # 设置机器人的y坐标
            odom.pose.pose.position.z = 0 # 设置机器人的z坐标为0
            odom.pose.pose.orientation = quaternion # 设置机器人的方向，用四元数表示
            odom.twist.twist.linear.x = vxy # 设置机器人的线速度
            odom.twist.twist.linear.y = 0 # 设置机器人的y方向线速度为0
            odom.twist.twist.angular.z = vth # 设置机器人的角速度

            self.odomPub.publish(odom) # 发布里程计数据
            
            if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): # 如果当前时间超过了上一次接收到速度控制指令的时间加上超时时间
                self.v_des_left = 0 # 将期望的左轮速度设为0
                self.v_des_right = 0 # 将期望的右轮速度设为0
                
            if self.v_left < self.v_des_left: # 如果当前的左轮速度小于期望的左轮速度
                self.v_left += self.max_accel # 增加左轮速度，以达到最大加速度
                if self.v_left > self.v_des_left: # 如果当前的左轮速度超过了期望的左轮速度
                    self.v_left = self.v_des_left # 将当前的左轮速度设为期望的左轮速度
            else: # 否则，如果当前的左轮速度大于期望的左轮速度
                self.v_left -= self.max_accel # 减小左轮速度，以达到最大加速度
                if self.v_left < self.v_des_left: # 如果当前的左轮速度低于了期望的左轮速度
                    self.v_left = self.v_des_left # 将当前的左轮速度设为期望的左轮速度
            
            if self.v_right < self.v_des_right: # 如果当前的右轮速度小于期望的右轮速度
                self.v_right += self.max_accel # 增加右轮速度，以达到最大加速度
                if self.v_right > self.v_des_right: # 如果当前的右轮速度超过了期望的右轮速度
                    self.v_right = self.v_des_right # 将当前的右轮速度设为期望的右轮速度
            else: # 否则，如果当前的右轮速度大于期望的右轮速度
                self.v_right -= self.max_accel # 减小右轮速度，以达到最大加速度
                if self.v_right < self.v_des_right: # 如果当前的右轮速度低于了期望的右轮速度
                    self.v_right = self.v_des_right # 将当前的右轮速度设为期望的右轮速度
            
            # Set motor speeds in encoder ticks per PID loop  根据每个PID循环中编码器刻度数，设置电机转速
            if not self.stopped: # 如果没有停止电机运行
                self.arduino.drive(self.v_left, self.v_right) # 调用Arduino对象的drive方法，传入左右轮转速
                
            self.t_next = now + self.t_delta # 更新下一次轮询时间，增加一个时间间隔量
 
    def stop(self):# 定义一个实例方法，用于停止电机
        self.stopped = True
        self.arduino.drive(0, 0)
            
    def cmdVelCallback(self, req): # 定义一个实例方法，用于处理速度命令的回调函数，参数为req
        # Handle velocity-based movement requests
        self.last_cmd_vel = rospy.Time.now()
        
        x = req.linear.x         # m/s
        th = req.angular.z       # rad/s

        if x == 0:
            # Turn in place
            right = th * self.wheel_track  * self.gear_reduction / 2.0
            left = -right
        elif th == 0:
            # Pure forward/backward motion
            left = right = x
        else:
            # Rotation about a point in space
            left = x - th * self.wheel_track  * self.gear_reduction / 2.0
            right = x + th * self.wheel_track  * self.gear_reduction / 2.0
            
        self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
        self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)

6.控制小车运动

使用teleop_twist_keyboard功能包

sudo apt install ros-melodic-teleop-twist-keyboard 

启动

rosrun teleop_twist_keyboard teleop_twist_keyboard.py 

根据提示运动小车。

控制原理

1. teleop_twist_keyboard功能包会将计算出的线速度和角速度封装成一个geometry_msgs/Twist类型的消息，并发布到/cmd_vel话题上。
2. ros小车运动的节点需要订阅/cmd_vel话题，并根据收到的Twist消息来控制底盘的电机转速或者发送控制指令给下位机。
3. 我们在 base_controller 节点订阅这个话题，接收速度数据，在转换成与底盘通信的格式，然后写入串口(poll函数)
4. 当小车底盘接收到串口发来的速度后，就控制电机运转，从而实现键盘控制小车的移动

7.控制机械臂

待…(后续使用QT实现)

五、传感器使用

1.激光雷达

这里使用的是思岚A1M8

安装驱动包

cd catkin_ws/src
git clone https://github.com/slamtec/rplidar_ros
cd ..
cd src/rplidar_ros/scripts/
./create_udev_rules.sh #默认的是ttyUSBX,但是由于每次可能不一样，直接将雷达端口映射(将端口 ttyUSBX 映射到 rplidar):

launch文件

<launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/rplidar"/>
  <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
  <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node>
</launch>

思岚雷达A1,2波特率一般是1152000 A3 ,4波特率是256000不能搞错了

一般性错误

• 波特率设置错误
• 端口号错误
• 端口没有权限
• 供电不足(换根线试试)

执行

roslaunch rplidar_ros rplidar.launch

再开一个终端执行

rviz

添加 LaserScan 插件即可显示雷达信息

2.USB相机

使用单目相机(便宜捏)

安装依赖

sudo apt-get install ros-melodic-usb-cam

launch文件

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video0" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

节点 usb_cam 用于启动相机，节点 image_view 以图形化窗口的方式显示图像数据，需要查看相机的端口并修改 usb_cam 中的 video_device 参数，并且如果将摄像头连接到了树莓派，且通过 ssh 远程访问树莓派的话，需要注释 image_view 节点，因为在终端中无法显示图形化界面。

执行

roslaunch usb_cam usb_cam-test.launch

再开一个终端执行

rviz

添加 LaserScan 插件即可显示相机信息

3.集成启动底盘、雷达、相机

cd catkin_ws/src
catkin_create_pkg car_start roscpp rospy std_msgs ros_arduino_python usb_cam rplidar_ros #新建功能包
cd ..
catkin_make
cd src/car_start/launch
vi car_start.launch

car_start.launch内容

<launch>
        <include file="$(find ros_arduino_python)/launch/arduino.launch" /> <!--底盘-->
        <include file="$(find usb_cam)/launch/usb_cam-test.launch" /> <!--相机-->
        <include file="$(find rplidar_ros)/launch/rplidar.launch" /> <!--雷达-->
</launch>

vi car_tf_start.launch #坐标转换

<launch>
    <include file="$(find mycar_start)/launch/start.launch" />    
    <node name="camera2basefootprint" pkg="tf2_ros" type="static_transform_publisher" args="0.08 0 0.1 0 0 0 /base_footprint /camera_link"/>
    <node name="rplidar2basefootprint" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0.1 0 0 0 /base_footprint /laser"/>
</launch>

以后直接运行roslaunch car_start car_tf_start.launch即可同时运行雷达相机底盘控制

六、SLAM建图

参考视频

1.准备工作

安装必要功能包

sudo apt install ros-melodic-gmapping #gmapping 包(用于构建地图)

sudo apt install ros-melodic-map-server #地图服务包(用于保存与读取地图)

sudo apt install ros-melodic-navigation # navigation 包(用于定位以及路径规划)

新建功能包，并导入依赖: gmapping map_server amcl move_base

cd catkin_ws/src
catkin_create_pkg nav roscpp rospy std_msgs gmapping map_server amcl move_base #新建功能包
cd ..
catkin_make

创建机器人模型相关的功能包

cd catkin_ws/src
catkin_create_pkg car_description urdf xacro
cd ..
catkin_make
cd /src/car_description
mkdir urdf && cd urdf
touch car.urdf.xacro #创建机器人模型，各种部件
touch car_base.urdf.xacro #机器人底盘实现
touch car_camera.urdf.xacro #机器人摄像头实现
touch car_laser.urdf.xacro #机器人雷达实现

car.urdf.xacro内容

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:include filename="car_base.urdf.xacro" />
    <xacro:include filename="car_camera.urdf.xacro" />
    <xacro:include filename="car_laser.urdf.xacro" />

</robot>

car_base.urdf.xacro

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="footprint_radius" value="0.001" />
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="${footprint_radius}" />
            </geometry>
        </visual>
    </link>


    <xacro:property name="base_radius" value="0.1" />
    <xacro:property name="base_length" value="0.08" />
    <xacro:property name="lidi" value="0.015" />
    <xacro:property name="base_joint_z" value="${base_length / 2 + lidi}" />
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.08" />
            </geometry>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <material name="baselink_color">
                <color rgba="1.0 0.5 0.2 0.5" />
            </material>
        </visual>

    </link>

    <joint name="link2footprint" type="fixed">
        <parent link="base_footprint"  />
        <child link="base_link" />
        <origin xyz="0 0 0.055" rpy="0 0 0" />
    </joint>



    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.015" />
    <xacro:property name="PI" value="3.1415927" />
    <xacro:property name="wheel_joint_z" value="${(base_length / 2 + lidi - wheel_radius) * -1}" />


    <xacro:macro name="wheel_func" params="wheel_name flag">

        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>

        </link>

        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />

            <origin xyz="0 ${0.1 * flag} ${wheel_joint_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>

    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />



    <xacro:property name="small_wheel_radius" value="0.0075" />
    <xacro:property name="small_joint_z" value="${(base_length / 2 + lidi - small_wheel_radius) * -1}" />

    <xacro:macro name="small_wheel_func" params="small_wheel_name flag">
        <link name="${small_wheel_name}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${small_wheel_radius}" />
                </geometry>

                <origin xyz="0 0 0" rpy="0 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>

        </link>

        <joint name="${small_wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${small_wheel_name}_wheel" />

            <origin xyz="${0.08 * flag} 0 ${small_joint_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro >
    <xacro:small_wheel_func small_wheel_name="front" flag="1"/>
    <xacro:small_wheel_func small_wheel_name="back" flag="-1"/>

</robot>

car_camera.urdf.xacro

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="camera_length" value="0.02" /> 
    <xacro:property name="camera_width" value="0.05" /> 
    <xacro:property name="camera_height" value="0.05" /> 
    <xacro:property name="joint_camera_x" value="0.08" />
    <xacro:property name="joint_camera_y" value="0" />
    <xacro:property name="joint_camera_z" value="${base_length / 2 + camera_height / 2}" />

    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0 0 0 0.8" />
            </material>
        </visual>
    </link>

    <joint name="camera2base" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${joint_camera_x} ${joint_camera_y} ${joint_camera_z}" rpy="0 0 0" />
    </joint>

</robot>

car_laser.urdf.xacro

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="support_radius" value="0.01" />
    <xacro:property name="support_length" value="0.15" />

    <xacro:property name="laser_radius" value="0.03" />
    <xacro:property name="laser_length" value="0.05" />

    <xacro:property name="joint_support_x" value="0" />
    <xacro:property name="joint_support_y" value="0" />
    <xacro:property name="joint_support_z" value="${base_length / 2 + support_length / 2}" />

    <xacro:property name="joint_laser_x" value="0" />
    <xacro:property name="joint_laser_y" value="0" />
    <xacro:property name="joint_laser_z" value="${support_length / 2 + laser_length / 2}" />

    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <material name="yellow">
                <color rgba="0.8 0.5 0.0 0.5" />
            </material>
        </visual>

    </link>

    <joint name="support2base" type="fixed">
        <parent link="base_link" />
        <child link="support"/>
        <origin xyz="${joint_support_x} ${joint_support_y} ${joint_support_z}" rpy="0 0 0" />
    </joint>
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.5" />
            </material>
        </visual>

    </link>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser"/>
        <origin xyz="${joint_laser_x} ${joint_laser_y} ${joint_laser_z}" rpy="0 0 0" />
    </joint>
</robot>

在launch文件加载机器人模型

cd catkin_ws/src/my_description/launch
vi car.launch

car.launc内容

<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find mycar_description)/urdf/car.urdf.xacro" />
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
</launch>

将模型加载再启动文件中

<launch>
        <include file="$(find ros_arduino_python)/launch/arduino.launch" />
        <include file="$(find usb_cam)/launch/usb_cam-test.launch" />
        <include file="$(find rplidar_ros)/launch/rplidar.launch" />
        <!-- 机器人模型加载文件 -->
        <include file="$(find mycar_description)/launch/car.launch" />
</launch>

2.SLAM建图

cd catkin_ws/src/nav
mkdir launch
touch gmapping.launch

<launch>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      <param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>
</launch>

执行

1.树莓派：roslaunch car_start start.launch；

2.树莓派：roslaunch nav gmapping.launch；

3.PC：rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.路径规划

1.地图存储

cd catkin_ws/src/nav
mkdir map
cd ..
cd launch
touch map_save.launch

map_save.launch

<launch>
    <arg name="filename" value="$(find nav)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

2.地图读取

cd catkin_ws/src/nav
mkdir map
cd ..
cd launch
touch map_server.launch

map_server.launch

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/>
</launch>

3.定位

cd catkin_ws/src/nav/launch
touch amcl.launch

amcl.launch

<launch>
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
    <param name="odom_alpha5" value="0.1"/>
    <param name="transform_tolerance" value="0.2" />
    <param name="gui_publish_rate" value="10.0"/>
    <param name="laser_max_beams" value="30"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="5000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="odom_alpha1" value="0.2"/>
    <param name="odom_alpha2" value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.8"/>
    <param name="odom_alpha4" value="0.2"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.2"/>
    <param name="update_min_a" value="0.5"/>

    <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
    <param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
    <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

  </node>
</launch>

4.move_base

cd catkin_ws/src/nav/launch
touch move_base.launch

move_base.launch

<launch>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find nav)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

5.编写配置文件

cd catkin_ws/src/nav
mkdir param
touch costmap_common_params.yaml
touch global_costmap_params.yaml
touch local_costmap_params.yaml
touch base_local_planner_params

costmap_common_params.yaml

该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

该文件用于全局代价地图参数设置

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

local_costmap_params.yaml

该文件用于局部代价地图参数设置

local_costmap:
  global_frame: odom #里程计坐标系
  robot_base_frame: base_footprint #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

base_local_planner_params

基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

6.集成

在nav的launch目录下新建一个launch文件nav.launch

<launch>
    <!-- 集成地图服务-->
    <include file="$(find nav)/launch/map_server.launch" />
    <!--集成定位实现-->
    <include file="$(find nav)/launch/amcl.launch" />
    <!--集成路径规划-->
    <include file="$(find nav)/launch/move_base.launch" />
</launch>

执行

1.树莓派：roslaunch mycar_start start.launch；

2.树莓派：roslaunch nav nav.launch；

3.PC启动rviz。

4.边规划边建图

就是直接集成前面的

在nav的launch目录下新建一个launch文件auto_slam.launch

<launch>
    <!-- 启动SLAM节点 -->
    <include file="$(find nav)/launch/gmapping.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find nav)/launch/move_base.launch" />
</launch>

执行

1.树莓派：roslaunch mycar_start start.launch；

2.树莓派：roslaunch nav auto_slam.launch；

3.PC：在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图了；

4.PC：最后可以使用 map_server 保存地图。

5.完整文件目录结构

七、后续安排

添加PCB打印电路替换杜邦线，3D打印芯片保护壳，红绿灯检测，车道检测，垃圾分类并通过机械臂收集垃圾，语言识别等。