基于 GPS 定位信息的 Pure-Pursuit 轨迹跟踪实车测试（1）

进行了多组实验，包括顺逆时针转向，直线+圆弧轨迹行驶，以及Pure-Pursuit 轨迹跟踪测试

代码修改

需要修改的代码并不多，主要对 gps_sensor 功能包和 purepursuit 代码的修改

gps_sensor 发布机器人实际运动轨迹，而不是在 purepursuit 中发布
gps_sensor 同时实现 mrobot_states_update 功能包的功能，发布机器人状态信息
purepursuit 将最终的控制指令发送给实际机器人，cmd_to_robot 代替 cmd_to_mrobot


#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Path.h>

#include <math.h>
#include <iostream>
#include <Eigen/Geometry>
#include <chrono>

using namespace std;

struct my_pose
{
    double latitude;
    double longitude;
    double altitude;
};

struct my_location
{
    double x;
    double y;
};

// 角度制转弧度制
double rad(double d)
{
    return d * M_PI / 180.0;
}

array<float, 4> calEulerToQuaternion(const float roll, const float pitch, const float yaw)
{
    array<float, 4> calQuaternion = {0.0f, 0.0f, 0.0f, 1.0f}; // 初始化四元数

    // 使用Eigen库来进行四元数计算
    Eigen::Quaternionf quat;
    quat = Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX()) *
           Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
           Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());

    calQuaternion[0] = quat.x();
    calQuaternion[1] = quat.y();
    calQuaternion[2] = quat.z();
    calQuaternion[3] = quat.w();

    return calQuaternion;
}

// 全局变量
ros::Publisher state_pub_;
nav_msgs::Path ros_path_;
ros::Publisher vel_pub;
ros::Publisher pose_pub;

const double EARTH_RADIUS = 6371.393; // 6378.137;地球半径

bool init = false;
my_pose init_pose;

// 计算速度
my_location pre_location;
my_location curr_location;
chrono::_V2::system_clock::time_point pre_time;
chrono::_V2::system_clock::time_point curr_time;

void gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps_msg_ptr)
{
    // 初始化
    if (!init)
    {
        init_pose.latitude = gps_msg_ptr->latitude;
        init_pose.longitude = gps_msg_ptr->longitude;
        init_pose.altitude = gps_msg_ptr->altitude;
        init = true;
    }
    else
    {
        // 计算相对位置
        double radLat1, radLat2, radLong1, radLong2, delta_lat, delta_long;
        radLat1 = rad(init_pose.latitude);
        radLong1 = rad(init_pose.longitude);
        radLat2 = rad(gps_msg_ptr->latitude);
        radLong2 = rad(gps_msg_ptr->longitude);
        // 计算x
        delta_lat = radLat2 - radLat1;
        delta_long = 0;
        double x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
        x = x * EARTH_RADIUS * 1000;

        // 计算y
        delta_lat = 0;
        delta_long = radLong1 - radLong2;
        double y = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
        y = y * EARTH_RADIUS * 1000;

        // 计算yaw
        array<float, 4> calQuaternion = calEulerToQuaternion(0.0, 0.0, gps_msg_ptr->altitude);

        // 发布轨迹
        ros_path_.header.frame_id = "world";
        ros_path_.header.stamp = ros::Time::now();
        geometry_msgs::PoseStamped pose;
        pose.header = ros_path_.header;
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.orientation.x = calQuaternion[0];
        pose.pose.orientation.y = calQuaternion[1];
        pose.pose.orientation.z = calQuaternion[2];
        pose.pose.orientation.w = calQuaternion[3];       
        ros_path_.poses.push_back(pose); 
        
        // 计算速度
        curr_location.x = x;
        curr_location.y = y;
        curr_time = chrono::system_clock::now();
        auto timeDifference = curr_time - pre_time;
        auto durationInMilliseconds = chrono::duration_cast<chrono::milliseconds>(timeDifference);
        double displacement = sqrt(pow(curr_location.x - pre_location.x, 2) + pow(curr_location.y - pre_location.y, 2));
        geometry_msgs::TwistStamped vel;
        // v = s / t
        vel.twist.linear.x = displacement / durationInMilliseconds.count() * 1000;
        // 更新信息
        pre_location = curr_location;
        pre_time = curr_time;
        
        cout << "位姿信息：" << endl;
        cout << x << "," << y << "," << gps_msg_ptr->altitude << endl;
        cout << "速度信息：" << endl;
        cout << vel.twist.linear.x << endl;
        cout << "---------" << endl;

        // 信息发布
        vel_pub.publish(vel);
        pose_pub.publish(pose);       
        state_pub_.publish(ros_path_);
    }
}

int main(int argc, char **argv)
{
    init = false;
    ros::init(argc, argv, "gps_subscriber");
    ros::NodeHandle n;
    ros::Subscriber pose_sub = n.subscribe("/robot_gps", 10, gpsCallback);

    state_pub_ = n.advertise<nav_msgs::Path>("/gps_path", 10);
    vel_pub = n.advertise<geometry_msgs::TwistStamped>("/center_velocity", 10);
    pose_pub = n.advertise<geometry_msgs::PoseStamped>("/center_pose", 10);

    ros::spin();
    return 0;
}

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include "cpprobotics_types.h"

#define PREVIEW_DIS 1.0 // 预瞄距离
#define Ld 0.55         // 轴距

using namespace std;
using namespace cpprobotics;

ros::Publisher purepersuit_;

const float target_vel = 0.2;
float carVelocity = 0;
float preview_dis = 0;
float k = 0.1;

// 计算四元数转换到欧拉角
std::array<float, 3> calQuaternionToEuler(const float x, const float y,
                                          const float z, const float w)
{
    std::array<float, 3> calRPY = {(0, 0, 0)};
    // roll = atan2(2(wx+yz),1-2(x*x+y*y))
    calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
    // pitch = arcsin(2(wy-zx))
    calRPY[1] = asin(2 * (w * y - z * x));
    // yaw = atan2(2(wx+yz),1-2(y*y+z*z))
    calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));

    return calRPY;
}

cpprobotics::Vec_f r_x_;
cpprobotics::Vec_f r_y_;

int pointNum = 0; // 保存路径点的个数
int targetIndex = pointNum - 1;

// 计算发送给模型车的转角
void poseCallback(const geometry_msgs::PoseStamped &currentWaypoint)
{
    auto currentPositionX = currentWaypoint.pose.position.x;
    auto currentPositionY = currentWaypoint.pose.position.y;
    auto currentPositionZ = 0.0;

    auto currentQuaternionX = currentWaypoint.pose.orientation.x;
    auto currentQuaternionY = currentWaypoint.pose.orientation.y;
    auto currentQuaternionZ = currentWaypoint.pose.orientation.z;
    auto currentQuaternionW = currentWaypoint.pose.orientation.w;

    std::array<float, 3> calRPY =
        calQuaternionToEuler(currentQuaternionX, currentQuaternionY,
                             currentQuaternionZ, currentQuaternionW);

    cout << currentPositionX << "," << currentPositionY << "," << calRPY[2] << endl;

    for (int i = pointNum - 1; i >= 0; --i)
    {
        float distance = sqrt(pow((r_x_[i] - currentPositionX), 2) +
                              pow((r_y_[i] - currentPositionY), 2));
        if (distance < preview_dis)
        {
            targetIndex = i + 1;
            break;
        }
    }

    if (targetIndex >= pointNum)
    {
        targetIndex = pointNum - 1;
    }

    float alpha =
        atan2(r_y_[targetIndex] - currentPositionY, r_x_[targetIndex] - currentPositionX) -
        calRPY[2];

    // 当前点和目标点的距离Id
    float dl = sqrt(pow(r_y_[targetIndex] - currentPositionY, 2) +
                    pow(r_x_[targetIndex] - currentPositionX, 2));

    // 发布小车运动指令及运动轨迹
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.z = 1.0;
    if (dl <= 0.2) // 离终点很近时停止运动
    {
        vel_msg.linear.x = 0;
        vel_msg.angular.z = 0;
        purepersuit_.publish(vel_msg);
    }
    else
    {
        float theta = atan(2 * Ld * sin(alpha) / dl);
        vel_msg.linear.x = target_vel;
        vel_msg.angular.z = theta;
        purepersuit_.publish(vel_msg);
    }
}

void velocityCall(const geometry_msgs::TwistStamped &carWaypoint)
{
    carVelocity = carWaypoint.twist.linear.x;
    // 预瞄距离计算
    preview_dis = k * carVelocity + PREVIEW_DIS;
}

void pointCallback(const nav_msgs::Path &msg)
{
    // 避免参考点重复赋值
    if (pointNum != 0)
    {
        return;
    }

    // geometry_msgs/PoseStamped[] poses
    pointNum = msg.poses.size();

    // 提前开辟内存
    r_x_.resize(pointNum);
    r_y_.resize(pointNum);
    for (int i = 0; i < pointNum; i++)
    {
        r_x_[i] = msg.poses[i].pose.position.x;
        r_y_[i] = msg.poses[i].pose.position.y;
    }
}

int main(int argc, char **argv)
{
    // 创建节点
    ros::init(argc, argv, "pure_pursuit");

    // 创建节点句柄
    ros::NodeHandle n;
    // 创建Publisher，发送经过pure_pursuit计算后的转角及速度
    purepersuit_ = n.advertise<geometry_msgs::Twist>("/cmd_vel", 20);

    ros::Subscriber splinePath = n.subscribe("/ref_path", 20, pointCallback);
    ros::Subscriber carVel = n.subscribe("/center_velocity", 20, velocityCall);
    ros::Subscriber carPose = n.subscribe("/center_pose", 20, poseCallback);
    ros::spin();
    return 0;
}

数据处理

原始的数据便于查阅，但不便于 MATLAB 进行读取分析，主要包括 ***_pub.txt 和 ***_path.txt 两种待处理数据，分别如下

$GPRMC,085750.20,A,3150.93719306,N,11717.59499143,E,0.071,252.6,161123,5.7,W,D*26

数据长度：83
latitude: 31.848953217667
longitude: 117.293249857167
heading_angle_deg: 252.600000000000
heading_angle_rad: 4.408701690538
heading_angle_rad: -1.874483616642
---------
$GPRMC,085750.30,A,3150.93719219,N,11717.59498178,E,0.297,264.0,161123,5.7,W,D*28

数据长度：83
latitude: 31.848953203167
longitude: 117.293249696333
heading_angle_deg: 264.000000000000
heading_angle_rad: 4.607669225265
heading_angle_rad: -1.675516081915
---------

位姿信息：
0.0169953,0.0210645,-2.24798
速度信息：
1.59198e-11
---------
位姿信息：
0.0183483,0.0200412,2.49058
速度信息：
0.0180464
---------

下面的命令首先使用**grep过滤包含"heading_angle_deg: "的行，然后使用awk**提取每行的第二个字段（即数字部分），最后将结果写入output.txt文件


grep "heading_angle_deg: " input.txt | awk '{print $2}' > output.txt

下面的命令使用**awk匹配包含"位姿信息:"的行，然后使用getline**读取下一行数据并打印出来，最后将结果写入output.txt文件

awk '/位姿信息:/ {getline; print}' input.txt > output.txt

处理前和处理后的文件目录如下

redwall@redwall-G3-3500:~/log/2023--11-16/raw$ tree
.
├── actual_path.txt
├── anticlock_path.txt
├── anticlock_pub.txt
├── clock_path.txt
├── clock_pub.txt
├── gps_path.txt
├── gps_pub.txt
├── lane_path.txt
└── lane_pub.txt

redwall@redwall-G3-3500:~/log/2023--11-16/processed$ tree
.
├── anticlock_path_pose.txt
├── anticlock_path_vel.txt
├── anticlock_pub_sift.txt
├── clock_path_pose.txt
├── clock_path_vel.txt
├── clock_pub_sift.txt
├── gps_path_pose.txt
├── gps_path_vel.txt
├── gps_pub_sift.txt
├── lane_path_pose.txt
├── lane_path_vel.txt
└── lane_pub_sift.txt