基于 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);// 计算xdelta_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;// 计算ydelta_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;// 计算yawarray<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 / tvel.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];// 当前点和目标点的距离Idfloat 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[] posespointNum = 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