运动路径规划，ROS发布期望运动轨迹

news2024/10/5 13:17:51

一、Python实现（推荐方法）

1.1代码cubic_spline_path.py

1.2使用方法

二、C++实现

参考博客

想让机器人/智能车无人驾驶，要有期望路径，最简单的是一条直线，或者是一条光滑曲线。

生成路径的方法有两种：

手动遥控机器人运动一段路径，并保存轨迹
人为设定几个关键点，拟合出直线或曲线

方法1通常是根据GNSS/INS位姿数据记录轨迹，可参考ROS之rviz显示GNSS/INS运动轨迹_可见一班的博客-CSDN博客

方法2是这篇博客讨论的内容。分别使用C++和Python在ROS下实现。

一、Python实现（推荐方法）

1.1代码cubic_spline_path.py

根据指定的插值算法 alg，创建一个插值函数 cubic_spline。如果 alg 为 “linear”，则使用线性插值函数 interp1d 创建插值函数；如果 alg 为 “cubic”，则使用三次样条插值函数 interp1d 创建插值函数。

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

import rospy
import rosparam
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import sys
import os
import yaml
import numpy as np
from scipy.interpolate import interp1d

# def interpolation(waypoint_list, alg="cubic"):
def interpolation(alg="linear"):
    ix = []
    iy = []

    temp_x = np.array([1,5,8,10])
    temp_y = np.array([1,4,9,10])
        
    cubic_spline = None
    if alg == "linear":
        cubic_spline = interp1d(temp_x, temp_y)
    elif alg == "cubic":
        cubic_spline = interp1d(temp_x, temp_y, kind='cubic')

    waypoint_x_start = temp_x[0]
    waypoint_x_end = temp_x[-1]
    length = int(abs(waypoint_x_end - waypoint_x_start) / 0.01)
    print(length)
    ix = np.linspace(waypoint_x_start, waypoint_x_end, num=length)
    iy = cubic_spline(ix)

    return ix, iy


if __name__ == '__main__':
    rospy.init_node("cubic_spline_path")

    waypoint_x, waypoint_y = interpolation()

    path_pub = rospy.Publisher('/spline_path', Path, queue_size=10)

    path = Path()
    for index in range(len(waypoint_x)):
        pose = PoseStamped()
        pose.pose.position.x = waypoint_x[index]
        pose.pose.position.y = waypoint_y[index]
        print(pose)
        path.poses.append(pose)
    path.header.frame_id = "map"
    path.header.stamp = rospy.Time.now()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        path_pub.publish(path)
        rate.sleep()
    rospy.spin()

1.2使用方法

在ROS工作空间中新建文件夹scripts
新建.py文件，复制上述代码
更改文件权限允许作为程序执行
终端执行 ./cubic_spline_path.py
打开rviz，订阅spline_path话题显示路径

二、C++实现

直线插值比较简单，三次样条插值比较复杂。

主函数中调用这个函数即可，传入参数为A、B点x，y坐标值，返回nav_msgs::Path路径。

// 线性插值路径
nav_msgs::Path LinearInterpolation(const double &Ax, const double &Ay, const double &Bx, const double &By, const double &stepsize)
{

  double x1 = Ax;
  double y1 = Ay;
  double z1 = 0;
  double x2 = Bx;
  double y2 = By;
  double z2 = 0;

  double step_size = stepsize;

  double dist = std::sqrt(std::pow((x2 - x1), 2) + std::pow((y2 - y1), 2) + std::pow((z2 - z1), 2));
  int num_steps = dist / step_size;

  for (int i = 0; i < num_steps; ++i)
  {
    double ratio = static_cast<double>(i) / num_steps;

    geometry_msgs::PoseStamped interpolated_pose;
    interpolated_pose.header.frame_id = "map";
    interpolated_pose.header.stamp = ros::Time::now();
    interpolated_pose.pose.position.x = x1 + ratio * (x2 - x1);
    interpolated_pose.pose.position.y = y1 + ratio * (y2 - y1);
    interpolated_pose.pose.position.z = z1 + ratio * (z2 - z1);

    interpolated_pose.pose.orientation.x = 1;
    interpolated_pose.pose.orientation.y = 0;
    interpolated_pose.pose.orientation.z = 0;
    interpolated_pose.pose.orientation.w = 0;

    scatter_path.poses.push_back(interpolated_pose);
  }

  return scatter_path;
}