树莓派小车激光雷达从0到准确定位python和c++都能实现（大学经验分享）

tp-try

于 2025-05-16 10:47:45 发布

阅读量654

点赞数 13

文章标签：经验分享 linux python c++ ubuntu 学习方法

本文链接：https://blog.csdn.net/2301_80300770/article/details/147952709

版权

我也是从五一零开始学习搞的小车，走了很多弯路，跟着我这个来肯定能做一个出来，后续可开发性很高，成品你只需要发送一个（1，1）就能直接到达目标位置

我也是从五一零开始学习搞的小车，走了很多弯路，跟着我这个来肯定能做一个出来，后续可开发性很高，成品你只需要发送一个（1，1）就能直接到达目标位置

硬件准备：

首先准备一个树莓派4B（有18.04镜像的，没有可以参考我前面发的树莓派考镜像）

一个L298N电机驱动模块

一个小车底盘（我这里用的是三轮，前轮是滚珠式，后轮是两个减速编码电机）

一个二维激光雷达（我这里用的是镭神N10，其他二维雷达也可以，只不过后续下载的包不一样）

硬件接线

我用的是drawio这个软件，接线图上传百度网盘了，百度网盘下载之后必须用drawio软件打开。或者网页版也可以打开，把下载的接线图文件拖到drawio这个网页也可以

通过网盘分享的文件：小车
链接: https://pan.baidu.com/s/1xlr8O69pWxlYu5DjvI4j3Q 提取码: haha
--来自百度网盘超级会员v3的分享

这个接线图和网盘上的差不多，只不过标记了左右电机，如果看不清楚就百度网盘下载

电机测试：

接线也搞好了之后就可以测试了，我这里用的是python，因为python不用编译，代码写完就能直接测试，所以比较方便，后续会改成C++，下面这个代码可以直接运行，他就是检测电机接线有没有问题的，电机转了代表硬件接线没问题了，编码器有值代表正常。如果运行没反应那就是，接线有问题，最重要的接线就是树莓派左下角那个GND一定要和L298N共地（用一个3S电池给L298N供电，树莓派那个GND也要接在L298N的供电负极）

#!/usr/bin/env python
# -*- coding: utf-8 -*- 
import RPi.GPIO as GPIO
import time

# ==== 引脚定义 ====
# 左电机控制引脚
IN1 = 22
IN2 = 23
ENA = 18  # PWM

# 右电机控制引脚
IN3 = 24
IN4 = 25
ENB = 19  # PWM

# 编码器引脚
ENCODER_LEFT = 17
ENCODER_RIGHT = 5

# 编码器计数
encoder_left_count = 0
encoder_right_count = 0

# ==== 编码器中断回调函数 ====
def encoder_left_callback(channel):
    global encoder_left_count
    encoder_left_count += 1

def encoder_right_callback(channel):
    global encoder_right_count
    encoder_right_count += 1

# ==== GPIO 初始化 ====
def setup():
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)

    # 电机控制引脚设为输出
    GPIO.setup(IN1, GPIO.OUT)
    GPIO.setup(IN2, GPIO.OUT)
    GPIO.setup(ENA, GPIO.OUT)

    GPIO.setup(IN3, GPIO.OUT)
    GPIO.setup(IN4, GPIO.OUT)
    GPIO.setup(ENB, GPIO.OUT)

    # 编码器引脚设为输入
    GPIO.setup(ENCODER_LEFT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    GPIO.setup(ENCODER_RIGHT, GPIO.IN, pull_up_down=GPIO.PUD_UP)

    # 编码器中断监听
    GPIO.add_event_detect(ENCODER_LEFT, GPIO.RISING, callback=encoder_left_callback)
    GPIO.add_event_detect(ENCODER_RIGHT, GPIO.RISING, callback=encoder_right_callback)

    # 初始化 PWM（100Hz）
    pwm_left = GPIO.PWM(ENA, 100)
    pwm_right = GPIO.PWM(ENB, 100)

    pwm_left.start(0)
    pwm_right.start(0)

    return pwm_left, pwm_right

# ==== 电机控制函数 ====
def motors_forward(pwm_left, pwm_right, speed):
    # 左轮前进
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    pwm_left.ChangeDutyCycle(speed)

    # 右轮前进
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_right.ChangeDutyCycle(speed)

def motors_stop(pwm_left, pwm_right):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    pwm_left.ChangeDutyCycle(0)

    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_right.ChangeDutyCycle(0)

# ==== 主函数 ====
def main():
    global encoder_left_count, encoder_right_count
    encoder_left_count = 0
    encoder_right_count = 0

    pwm_left, pwm_right = setup()

    print("两个电机开始转动...")
    motors_forward(pwm_left, pwm_right, 50)

    time.sleep(3)  # 运行 3 秒

    motors_stop(pwm_left, pwm_right)
    print("两个电机停止")
    print("左编码器计数：", encoder_left_count)
    print("右编码器计数：", encoder_right_count)

    GPIO.cleanup()

if __name__ == "__main__":
    main()

这是我用VScode运行的结果

雷达驱动：

这里注意了，每个人的雷达可能不一样，记得找自己的商家要雷达资料，我这里用的是镭神N10

雷达驱动包同样放在百度网盘里面

首先把雷达插在树莓派上面，输入这个指令看雷达连接的串口

ls /dev/tty*

找到之后，打开雷达驱动的包，打开，把包里面的串口号改成自己雷达的串口号我这里是/dev/ttyCH343USB0

改完了之后运行这个launch文件，这是你的雷达驱动

roslaunch lslidar_driver lslidar_serial.launch

看到这样的输出，就代表你的雷达驱动没问题了

RVIZ获取雷达数据：

首先是运行雷达驱动，记住每次都要运行，然后打开rviz可视化

rosrun rviz rviz

雷达驱动和rviz都运行了之后就可以看数据了，首先把左上角的map改成自己雷达的地图，我的是laser，然后左下角add添加By topic的laserscan。选好了之后就可以看到我这个类似的点云图了。如果不知道自己的雷达该怎么改，可以看自己的雷达驱动文件，前面说了lslidar_serial.launch,这个文件里面都有的

如果看到了自己雷达的数据，证明这一步成功了

hector_slam建图：

前面的按照我的步骤来就不会出错，这一步才是最烦人的，我也是搞了一天。

一定要知道建图分两种，一种是实时建图，这个就是你每次打开的新的rviz都是一个新的地图，如果换了环境也可以用。另一种就是静态建图，这是规定了地图的，也就是实时建图完成了之后保存地图，之后每次用的地图就是之前保存的静态地图。静态地图精度高一些，但是地图固定了，所以我建议的是实时地图，我这里采用的也是实时建图。

首先是要明确，这是实物，不是仿真。然后就可以下载hector_slam的ros包了

sudo apt-get update
sudo apt-get install ros-melodic-hector-slam

下载完了就放在src下面，然后打开里面的tutorial.launch。这里注意绿色的部分就是仿真时间，要么像我一样把他注释掉，要么改成false

改完了之后还有一个地方就是坐标系的转化，这里官方的lsx10包里面有个map->nav的地图，但是这里地图没用到nav，必须把这个中间地图去掉，才能把地图连起来map->base_link->laser。如果不去掉就是map->nav断开base_link-laser。

找到我这个mapping_default.launch文件，然后把这个pub_map_odom_transform参数改成false

改完之后就是需要静态转换了，我下面已经写好了这个launch文件，里面包括了雷达初始化，第一个就是雷达初始化，然后下面就是坐标静态转换，后面那个rviz可视化可以关掉，但是前面调试的时候还是加上，方便看

<?xml version="1.0"?>
<launch>
  <!-- 启动激光雷达驱动程序 -->
  <include file="$(find lslidar_driver)/launch/lslidar_serial.launch">
    <!-- 如果需要，可以在这里添加激光雷达驱动程序的参数 -->
  </include>



  <!-- 发布从 base_link 到 laser 的静态变换 -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_laser" 
        args="0 -0.2 0 0 0 0 base_link laser 100" />

        

<node pkg="tf" type="static_transform_publisher" name="map_to_base_link" 
      args="0 0 0 0 0 0 map base_link 100" />

  <!-- 启动 Hector SLAM -->
  <!-- <include file="$(find car)/launch/my_slam.launch"/> -->
  <include file="$(find hector_slam_launch)/launch/tutorial.launch"/>

    <!-- 启动 Hector SLAM 配置文件中的参数 -->
  <param name="base_frame" value="base_link" />
  <param name="map_frame" value="map" />

  <!-- 启动 RViz 用于可视化 -->
  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/> -->
</launch>

如果运行我这个launch文件出现下面这个报错，那就是nav那个没改，把他改成false

为了确保坐标系是正确的，运行我这个launch文件，然后输入下面的指令。他会生成一个坐标系转换图，在你的主目录下面，然后看到map->base_link->laser就说明是对的，也实时建图启动成功了

如果说运行像我这个一样，那就是雷达的地图转换的一些配置还有问题，下面这个别看地图是有了，但是其实坐标系是有问题的，我也是找了好久，这个不解决的话，后续路径规划，避障时会有问题的，这就是前面那个参数没改成false的问题，改了就好了

这些配置都搞好了之后基本上就可以了，然后就可以写代码控制小车了，我这里是比较简单的控制，后续都可以扩展

python控制：

#!/usr/bin/env python
# -*- coding: utf-8 -*- 

import rospy
from geometry_msgs.msg import Twist, PoseWithCovarianceStamped, PoseStamped
import RPi.GPIO as GPIO
import time
import math

# ==================== 电机控制引脚 ====================
LEFT_IN1 = 22
LEFT_IN2 = 23
LEFT_EN  = 18

RIGHT_IN1 = 24
RIGHT_IN2 = 25
RIGHT_EN  = 19

PWM_FREQ = 10000

# ==================== 编码器引脚 ====================
ENC_L_A = 17
ENC_L_B = 27
ENC_R_A = 5
ENC_R_B = 6

# ==================== 参数 ====================
WHEEL_BASE = 0.129
WHEEL_DIAMETER = 0.047
PULSES_PER_REV = 266
SCALE_PWM = 30

# ==================== 全局变量 ====================
left_pulse_count = 0
right_pulse_count = 0

current_x = 0.0
current_y = 0.0
current_theta = 0.0

# ==================== GPIO 初始化 ====================
def setup_gpio():
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)

    motor_pins = [LEFT_IN1, LEFT_IN2, LEFT_EN, RIGHT_IN1, RIGHT_IN2, RIGHT_EN]
    for pin in motor_pins:
        GPIO.setup(pin, GPIO.OUT)

    GPIO.setup(ENC_L_A, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    GPIO.setup(ENC_R_A, GPIO.IN, pull_up_down=GPIO.PUD_UP)

    global left_pwm, right_pwm
    left_pwm = GPIO.PWM(LEFT_EN, PWM_FREQ)
    right_pwm = GPIO.PWM(RIGHT_EN, PWM_FREQ)
    left_pwm.start(0)
    right_pwm.start(0)

    GPIO.add_event_detect(ENC_L_A, GPIO.BOTH, callback=encoder_callback_L)
    GPIO.add_event_detect(ENC_R_A, GPIO.BOTH, callback=encoder_callback_R)

# ==================== 编码器回调 ====================
def encoder_callback_L(channel):
    global left_pulse_count
    left_pulse_count += 1

def encoder_callback_R(channel):
    global right_pulse_count
    right_pulse_count += 1

# ==================== 电机控制 ====================
def set_motor(left_speed, right_speed):
    GPIO.output(LEFT_IN1, GPIO.HIGH if left_speed >= 0 else GPIO.LOW)
    GPIO.output(LEFT_IN2, GPIO.LOW if left_speed >= 0 else GPIO.HIGH)
    left_pwm.ChangeDutyCycle(min(abs(left_speed), 100))

    GPIO.output(RIGHT_IN1, GPIO.HIGH if right_speed >= 0 else GPIO.LOW)
    GPIO.output(RIGHT_IN2, GPIO.LOW if right_speed >= 0 else GPIO.HIGH)
    right_pwm.ChangeDutyCycle(min(abs(right_speed), 100))

def stop_motor():
    left_pwm.ChangeDutyCycle(0)
    right_pwm.ChangeDutyCycle(0)
    GPIO.output(LEFT_IN1, GPIO.LOW)
    GPIO.output(LEFT_IN2, GPIO.LOW)
    GPIO.output(RIGHT_IN1, GPIO.LOW)
    GPIO.output(RIGHT_IN2, GPIO.LOW)
    rospy.loginfo("电机已停止")

# ==================== 工具函数 ====================
def get_distance(x1, y1, x2, y2):
    return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)

def get_angle_to_goal(x1, y1, x2, y2):
    return math.atan2(y2 - y1, x2 - x1)

def normalize_angle(angle):
    while angle > math.pi:
        angle -= 2 * math.pi
    while angle < -math.pi:
        angle += 2 * math.pi
    return angle

# ==================== 旋转一定角度（正数左转，负数右转） ====================
def rotate_angle(target_angle_rad):
    # 左右转速度设置（正：左转，负：右转）
    direction = 1 if target_angle_rad > 0 else -1
    duty = 40  # 占空比
    duration = abs(target_angle_rad) / (math.pi / 2) * 1.2  # 简化估算，90度约1.2s

    rospy.loginfo("开始旋转 %.2f 弧度，方向：%s", target_angle_rad, "左" if direction > 0 else "右")
    if direction > 0:
        set_motor(-duty, duty)
    else:
        set_motor(duty, -duty)

    time.sleep(duration)
    stop_motor()
    rospy.sleep(0.5)

# ==================== 控制机器人到达目标点 ====================
def move_to_goal(goal_x, goal_y):
    global current_x, current_y, current_theta, left_pulse_count, right_pulse_count

    # 计算方向
    goal_angle = get_angle_to_goal(current_x, current_y, goal_x, goal_y)
    
    # 旋转对准目标方向
    max_step = 0.085
    angle_diff = normalize_angle(goal_angle - current_theta)

    while abs(angle_diff) > 0.1745:
        step = max_step if angle_diff > 0 else -max_step
        if abs(angle_diff) < abs(step):
            step = angle_diff
        rotate_angle(step)
        angle_diff = normalize_angle(goal_angle - current_theta)

    # ========== 开始前进 ==========
    rospy.loginfo("开始前进")
    
    # 记录初始编码器值
    start_left = left_pulse_count
    start_right = right_pulse_count

    # 理想目标距离
    target_distance = get_distance(current_x, current_y, goal_x, goal_y)
    wheel_circumference = math.pi * WHEEL_DIAMETER
    target_pulses = (target_distance / wheel_circumference) * PULSES_PER_REV

    base_pwm = 20 
    pwm_diff = 2  

    rate = rospy.Rate(20)
    while not rospy.is_shutdown():
        current_left = left_pulse_count - start_left
        current_right = right_pulse_count - start_right
        avg_pulse = (current_left + current_right) / 2.0

        # 判断是否到达目标距离
        if avg_pulse >= target_pulses or get_distance(current_x, current_y, goal_x, goal_y) < 0.15:
            break

        # 编码器差值调节PWM
        diff = current_left - current_right
        if diff > 5:
            # 左边快了
            left_pwm_val = base_pwm - pwm_diff
            right_pwm_val = base_pwm + pwm_diff
        elif diff < -5:
            # 右边快了
            left_pwm_val = base_pwm + pwm_diff
            right_pwm_val = base_pwm - pwm_diff
        else:
            # 两边基本一致
            left_pwm_val = base_pwm
            right_pwm_val = base_pwm

        set_motor(left_pwm_val, right_pwm_val)
        rate.sleep()

    stop_motor()
    rospy.loginfo("到达目标点")

# ==================== 订阅 SLAM 位姿 ====================
def pose_callback(msg):
    global current_x, current_y, current_theta

    current_x = msg.pose.position.x
    current_y = msg.pose.position.y
    orientation_q = msg.pose.orientation
    _, _, current_theta = euler_from_quaternion(orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w)

def euler_from_quaternion(x, y, z, w):
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll_x = math.atan2(t0, t1)

    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch_y = math.asin(t2)

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)

    return roll_x, pitch_y, yaw_z

# ==================== 初始位姿 ====================
def publish_initial_pose():
    pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=10)
    rospy.sleep(1.0)

    pose = PoseWithCovarianceStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "map"
    pose.pose.pose.position.x = 0.0
    pose.pose.pose.position.y = 0.0
    pose.pose.pose.orientation.w = 1.0

    pub.publish(pose)
    rospy.loginfo("已发布初始位姿")

# ==================== 主函数 ====================
def motor_driver_node():
    rospy.init_node('moter')
    setup_gpio()
    publish_initial_pose()

    rospy.Subscriber('/slam_out_pose', PoseStamped, pose_callback)

    rospy.loginfo("motor_driver_node 已启动，监听 /slam_out_pose")

    move_to_goal(1.0, 1.0)

    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
    finally:
        stop_motor()
        GPIO.cleanup()

if __name__ == '__main__':
    motor_driver_node()

这个是全写在一块的，方便初学者直接运行，上面的代码直接复制粘贴就能直接运行，唯一需要注意的是这里使用了GPIO，如果没下载的话需要下载一个RPI.GPIO，然后就能直接运行了，这是一个跑到（1，1）的测试代码

C++控制：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <wiringPi.h>
#include <softPwm.h>
#include <cmath>
#include <iostream>
#include <tf/tf.h>

using namespace std;

// ==================== 电机控制引脚 ====================
#define LEFT_IN1  22
#define LEFT_IN2  23
#define LEFT_EN   18

#define RIGHT_IN1 24
#define RIGHT_IN2 25
#define RIGHT_EN  19

#define PWM_FREQ 10000

// ==================== 编码器引脚 ====================
#define ENC_L_A 17
#define ENC_L_B 27
#define ENC_R_A 5
#define ENC_R_B 6

// ==================== 参数 ====================
#define WHEEL_BASE 0.129
#define WHEEL_DIAMETER 0.047
#define PULSES_PER_REV 266
#define SCALE_PWM 30

// ==================== 全局变量 ====================
volatile int left_pulse_count = 0;
volatile int right_pulse_count = 0;

double current_x = 0.0;
double current_y = 0.0;
double current_theta = 0.0;

// ==================== 编码器回调 ====================
void encoder_callback_L() {
    left_pulse_count++;
}

void encoder_callback_R() {
    right_pulse_count++;
}

// ==================== GPIO 初始化 ====================
void setup_gpio() {
    wiringPiSetupGpio(); // 使用 BCM 引脚编号

    // 设置电机引脚
    pinMode(LEFT_IN1, OUTPUT);
    pinMode(LEFT_IN2, OUTPUT);
    pinMode(LEFT_EN, OUTPUT);
    pinMode(RIGHT_IN1, OUTPUT);
    pinMode(RIGHT_IN2, OUTPUT);
    pinMode(RIGHT_EN, OUTPUT);

    // 设置编码器引脚
    pinMode(ENC_L_A, INPUT);
    pinMode(ENC_R_A, INPUT);
    pullUpDnControl(ENC_L_A, PUD_UP);
    pullUpDnControl(ENC_R_A, PUD_UP);

    // 启动 PWM 控制
    softPwmCreate(LEFT_EN, 0, 100);
    softPwmCreate(RIGHT_EN, 0, 100);

    // 编码器事件检测
    wiringPiISR(ENC_L_A, INT_EDGE_BOTH, encoder_callback_L);
    wiringPiISR(ENC_R_A, INT_EDGE_BOTH, encoder_callback_R);
}

// // ==================== 编码器回调 ====================
// void encoder_callback_L() {
//     left_pulse_count++;
// }

// void encoder_callback_R() {
//     right_pulse_count++;
// }

// ==================== 电机控制 ====================
void set_motor(int left_speed, int right_speed) {
    digitalWrite(LEFT_IN1, left_speed >= 0 ? HIGH : LOW);
    digitalWrite(LEFT_IN2, left_speed >= 0 ? LOW : HIGH);
    softPwmWrite(LEFT_EN, std::min(abs(left_speed), 100));

    digitalWrite(RIGHT_IN1, right_speed >= 0 ? HIGH : LOW);
    digitalWrite(RIGHT_IN2, right_speed >= 0 ? LOW : HIGH);
    softPwmWrite(RIGHT_EN, std::min(abs(right_speed), 100));
}

void stop_motor() {
    softPwmWrite(LEFT_EN, 0);
    softPwmWrite(RIGHT_EN, 0);
    digitalWrite(LEFT_IN1, LOW);
    digitalWrite(LEFT_IN2, LOW);
    digitalWrite(RIGHT_IN1, LOW);
    digitalWrite(RIGHT_IN2, LOW);
    ROS_INFO("电机已停止");
}

// ==================== 工具函数 ====================
double get_distance(double x1, double y1, double x2, double y2) {
    return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

double get_angle_to_goal(double x1, double y1, double x2, double y2) {
    return atan2(y2 - y1, x2 - x1);
}

double normalize_angle(double angle) {
    while (angle > M_PI) {
        angle -= 2 * M_PI;
    }
    while (angle < -M_PI) {
        angle += 2 * M_PI;
    }
    return angle;
}

// ==================== 旋转一定角度（正数左转，负数右转） ====================
void rotate_angle(double target_angle_rad) {
    int direction = (target_angle_rad > 0) ? 1 : -1;
    int duty = 40;
    double duration = fabs(target_angle_rad) / (M_PI / 2) * 1.2;

    ROS_INFO("开始旋转 %.2f 弧度，方向：%s", target_angle_rad, (direction > 0) ? "左" : "右");

    if (direction > 0) {
        set_motor(-duty, duty);
    } else {
        set_motor(duty, -duty);
    }

    delay(duration * 1000);  // 延时的单位是毫秒
    stop_motor();
    ros::Duration(0.5).sleep();
}

// ==================== 控制机器人到达目标点 ====================
void move_to_goal(double goal_x, double goal_y) {
    double goal_angle = get_angle_to_goal(current_x, current_y, goal_x, goal_y);
    double max_step = 0.085;
    double angle_diff = normalize_angle(goal_angle - current_theta);

    while (fabs(angle_diff) > 0.1745) {  // 10度
        double step = (angle_diff > 0) ? max_step : -max_step;
        if (fabs(angle_diff) < fabs(step)) {
            step = angle_diff;
        }
        rotate_angle(step);
        angle_diff = normalize_angle(goal_angle - current_theta);
    }

    ROS_INFO("开始前进");

    int start_left = left_pulse_count;
    int start_right = right_pulse_count;

    double target_distance = get_distance(current_x, current_y, goal_x, goal_y);
    double wheel_circumference = M_PI * WHEEL_DIAMETER;
    double target_pulses = (target_distance / wheel_circumference) * PULSES_PER_REV;

    int base_pwm = 20;
    int pwm_diff = 2;

    ros::Rate rate(20);
    while (ros::ok()) {
        int current_left = left_pulse_count - start_left;
        int current_right = right_pulse_count - start_right;
        double avg_pulse = (current_left + current_right) / 2.0;

        if (avg_pulse >= target_pulses || get_distance(current_x, current_y, goal_x, goal_y) < 0.15) {
            break;
        }

        int diff = current_left - current_right;
        int left_pwm_val = (diff > 5) ? base_pwm - pwm_diff : base_pwm + pwm_diff;
        int right_pwm_val = (diff < -5) ? base_pwm - pwm_diff : base_pwm + pwm_diff;

        set_motor(left_pwm_val, right_pwm_val);
        rate.sleep();
    }

    stop_motor();
    ROS_INFO("到达目标点");
}

// ==================== 订阅 SLAM 位姿 ====================
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    current_x = msg->pose.position.x;
    current_y = msg->pose.position.y;
    current_theta = tf::getYaw(msg->pose.orientation);
}

// ==================== 初始位姿 ====================
void publish_initial_pose(ros::Publisher& pub) {
    geometry_msgs::PoseWithCovarianceStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "map";
    pose.pose.pose.position.x = 0.0;
    pose.pose.pose.position.y = 0.0;
    pose.pose.pose.orientation.w = 1.0;
    pub.publish(pose);
    ROS_INFO("已发布初始位姿");
}

// ==================== 主函数 ====================
int main(int argc, char** argv) {
    ros::init(argc, argv, "motor_driver_node");
    ros::NodeHandle nh;

    setup_gpio();

    ros::Publisher pub_initial_pose = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
    publish_initial_pose(pub_initial_pose);

    ros::Subscriber sub_pose = nh.subscribe("/slam_out_pose", 10, pose_callback);

    ROS_INFO("motor_driver_node 已启动，监听 /slam_out_pose");

    move_to_goal(1.0, 1.0);

    ros::spin();
    stop_motor();
    return 0;
}

这个我也是搞了一段时间的，因为C++的GPIO控制我感觉还是WiningPi好用，但是他这个版本的问题也是让人头疼。我这里用的是WiningPi。需要克隆，这里是下载不了的，所以运行下面的指令

cd ~
git clone https://github.com/WiringPi/WiringPi.git
cd WiringPi
git checkout 2.50
./build
gpio -v

如果说克隆失败了或者连接超时了没关系，多试几次，因为没有梯子的缘故，要是有梯子最好。这里一步一步按照我这个来安装就不会出错，我之前搞了半天2.52版本，结果不行我用的2.50版本，最后一条指令时检查版本号，如果没问题那就是没问题了

这里搞完了之后还要在cmakelist里面加连接WiningPi的配置，这个可以直接问chartgpt或者网上找官方配置，因为我的不太一样，所以就没发出来，不过这一步很简单，一般经常用树莓派引脚的WiningPi都是配置了的。所以到这就全部结束了

总结：

这里简单说一下测试的代码，小车是先转弯，每次转一定的角度，然后根据编码器两边相同值走直线到目标位置的，所以小车前轮胎最好是滚珠的，然后雷达判断到目标点了吗，也可以降低小车速度，因为小车有惯性，所以就算到了目标位置还是会往前走一些距离。因为我也是做完了有一段时间了，所以估计中间有些问题已经记不清楚了，可以提问，我是做出来了的，所以这个方法绝对可行。最后，我把小车接线图，lsx10雷达驱动包，hector_slam都放到百度网盘，需要自取，但是我建议学习阶段还是自己来