【运动规划算法项目实战】如何加载csv文件的路径信息（附ROS C++代码）

---

简介

在运动规划算法项目中，路径规划是非常重要的一环。在实际应用中，我们通常需要将预先规划好的路径以某种方式加载到程序中进行后续处理和运动控制。而CSV文件作为一种常见的数据交换格式，也被广泛用于存储路径信息。因此，学会如何加载CSV文件中的路径信息将是非常有帮助的。

在ROS中，我们可以使用C++和Python等多种编程语言来加载CSV文件。其中，C++作为ROS的主要开发语言，具有良好的性能和稳定性，更适合在实际应用中使用。下面我们将以C++为例，介绍如何加载CSV文件中的路径信息。

---

一、CSV简介

首先简单介绍一下什么是CSV？CSV（Comma-Separated Values）文件是一种常见的数据存储格式，其主要特点是以逗号为分隔符，将数据存储在纯文本文件中。CSV文件可被许多软件和编程语言支持，如Microsoft Excel、Google Sheets、Python、R等，因此被广泛应用于数据交换和数据分析领域。

1.1 CSV文件格式

CSV文件的格式相对简单，其主要特点是每行数据对应于一条记录，每个字段之间以逗号分隔。在某些情况下，也可以使用其他符号作为分隔符，如制表符（Tab）或分号。

下面是一个简单的CSV文件例子：

Name, Age, Gender
Tom, 24, Male
Lily, 28, Female
John, 31, Male

这个文件包含三个字段（Name、Age、Gender）和三个记录（Tom、Lily、John）。

1.2 CSV文件与其他格式的比较

相对于其他数据存储格式，CSV文件有以下优点：

1. 简单明了：CSV文件的格式相对简单，易于理解和编辑。

2. 通用性：CSV文件可被许多软件和编程语言支持，因此可以被用于数据交换和数据分析等多种场景。

3. 跨平台：CSV文件是基于文本的，因此可以在不同操作系统和编程环境下使用。

与其他格式相比，CSV文件的不足之处包括：

1. 无法存储复杂数据类型：CSV文件只能存储简单的文本和数字类型数据，无法存储复杂数据类型，如图片、视频等。

2. 无法处理大规模数据：如果数据量非常大，CSV文件可能会变得非常庞大，这会导致读写速度下降和存储问题。

1.3 应用场景

CSV文件适用于多种数据存储和交换场景，例如：

1. 数据分析：CSV文件可以用于存储和交换数据，以便进行数据分析、数据挖掘等工作。

2. 数据导出和导入：CSV文件是许多软件和网站常用的数据导入和导出格式，如Microsoft Excel、Google Sheets、MySQL等。

3. 数据备份：将数据存储为CSV文件可以方便地进行数据备份和还原。

CSV文件是一种常见的数据存储格式，其主要特点是简单明了、通用性强、跨平台，适用于多种数据存储和交换场景。虽然其无法存储复杂数据类型和处理大规模数据，但由于其简单易用的特点，仍被广泛应用于数据交换和数据分析领域。

---

二、路径消息类型

首先，我们需要在ROS中定义一个路径消息类型，这个消息类型可以用来表示路径的起点、终点和路径上的所有中间点。路径消息类型通常包含一个Header字段和一个位于poses字段中的序列，该序列包含路径上的所有中间点。在ROS中，路径消息类型通常是nav_msgs::Path 。

它包含了以下几个重要的信息：

• std_msgs/Header header: 该路径消息的头部信息，包括时间戳和坐标系信息。
• std::vector<geometry_msgs/PoseStamped> poses: 该路径消息包含的位姿信息的列表，是路径的关键部分。每个位姿信息是一个 geometry_msgs/PoseStamped 类型，它包含以下几个重要的信息：
  • std_msgs/Header header: 该位姿信息的头部信息，与 nav_msgs::Path 中的 header 相似。
  • geometry_msgs/Pose pose: 位姿信息，包括位置和方向。位置由一个 geometry_msgs/Point 类型表示，包括x、y和z三个坐标轴；方向由一个 geometry_msgs/Quaternion 类型表示，包括四元数的四个分量。

通过 nav_msgs::Path 类型消息中包含的这些信息，我们可以方便地表示出路径规划的结果，并用于实际的导航和控制中。

出于简单演示的目的，我们的CSV文件中包含的路径点只考虑了XY坐标信息，完整的路径包含其他信息会在后续的内容再进行说明。

---

三、代码实现

3.1 读取CSV文件步骤

接下来，我们需要读取CSV文件中的路径信息，并将其转换为路径消息类型。这一过程通常包括以下几个步骤：

1. 打开CSV文件，并读取其中的路径信息。在C++中，我们可以使用std::ifstream来打开文件，然后使用std::getline逐行读取文件中的数据。

2. 解析CSV文件中的路径信息，并将其存储为一个序列。在C++中，我们可以使用std::stringstream来将字符串解析为数值类型。

3. 将路径信息转换为路径消息类型。在C++中，我们可以使用nav_msgs::Path消息类型中的geometry_msgs::PoseStamped来表示路径上的中间点，然后将所有的中间点存储到nav_msgs::Path消息类型的poses字段中。

最后，我们需要将加载好的路径消息类型发布到ROS系统中。这样，我们就可以使用其他节点订阅该路径消息，并进行后续的路径规划和运动控制。

3.2 核心代码

waypoint_loader.cpp代码

#include <waypoint_loader/waypoint_loader.h>

WaypointLoader::WaypointLoader(ros::NodeHandle nh, ros::NodeHandle pnh)
    : nh_(nh), pnh_(pnh)
{
    auto paramOrDefault = [this](const std::string& key, const std::string& defaultValue) {
        std::string value;
        if (!pnh_.getParam(key, value)) {
            ROS_WARN_STREAM("Parameter " << key << " not set, using default: " << defaultValue);
            value = defaultValue;
        }
        return value;
    };

    path_topic_ = paramOrDefault("path_topic", "/waypoints_raw");
    waypoints_path_ = paramOrDefault("waypoints_csv", "/tmp/waypoints.csv");
    map_frame_ = paramOrDefault("map_frame", "odom");

    wps_pub_ = nh_.advertise<nav_msgs::Path>(path_topic_, 10);
    LoadWaypointsArray_();
    wp_publish_thread_ = std::thread(&WaypointLoader::PublishWaypoints_, this);
}

WaypointLoader::~WaypointLoader()
{
    if (wp_publish_thread_.joinable()) {
        wp_publish_thread_.join();
    }
}

void WaypointLoader::PublishWaypoints_()
{
    ros::Rate loop_rate(10);

    while (ros::ok()) {
        wps_pub_.publish(wps_);
        loop_rate.sleep();
    }
}

void WaypointLoader::LoadWaypointsArray_()
{
    current_time_ = ros::Time::now();
    wps_.header.stamp = current_time_;
    wps_.header.frame_id = map_frame_;

    std::ifstream ifs(waypoints_path_);

    if (!ifs.is_open()) {
        ROS_ERROR_STREAM("Failed to open file: " << waypoints_path_);
        return;
    }

    std::string line;
    std::getline(ifs, line); // Remove first line

    while (std::getline(ifs, line)) {
        geometry_msgs::PoseStamped wp;
        LoadWaypoint_(line, &wp);
        wps_.poses.push_back(wp);
    }
}

void WaypointLoader::LoadWaypoint_(const std::string& line, geometry_msgs::PoseStamped* wp)
{
    std::istringstream stream(line);
    std::string column;

    for (auto& pos : { &wp->pose.position.x, &wp->pose.position.y, &wp->pose.position.z }) {
        std::getline(stream, column, ',');
        *pos = std::stod(column);
    }

    std::getline(stream, column, ',');
    double yaw = std::stod(column);
    tf2::Quaternion quat;
    quat.setRPY(0.0, 0.0, yaw);
    wp->pose.orientation = tf2::toMsg(quat);

    wp->header.stamp = current_time_;
    wp->header.frame_id = map_frame_;
}

这段代码是一个用于加载路径点数据并发布为ROS消息的类WaypointLoader。以下是对代码的简要分析：

• 构造函数WaypointLoader::WaypointLoader：接受两个ros::NodeHandle参数，并进行初始化。在构造函数中，使用了C++11中的lambda表达式来处理参数的获取。lambda表达式通过捕获类成员变量pnh_，避免了重复的代码，如果参数未设置，则使用默认值。

• 析构函数WaypointLoader::~WaypointLoader：在对象销毁时，如果wp_publish_thread_可加入，则等待线程的结束。

• 成员函数WaypointLoader::PublishWaypoints_：在一个循环中，将路径点消息wps_发布到指定的话题path_topic_。

• 成员函数WaypointLoader::LoadWaypointsArray_：从CSV文件中加载路径点数据，并将其存储到wps_中。路径点数据的格式为"x,y,z,yaw"，逐行读取CSV文件并解析每个路径点，创建一个geometry_msgs::PoseStamped对象，并将其添加到wps_.poses中。

• 成员函数WaypointLoader::LoadWaypoint_：解析单个路径点的数据行，并将其加载到geometry_msgs::PoseStamped对象中。该函数将CSV行分割为逗号分隔的值，并解析x、y、z坐标和偏航角，将其存储到geometry_msgs::PoseStamped对象中的相应字段中。

waypoint_loader_node.cpp代码

#include <waypoint_loader/waypoint_loader.h>

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "waypoint_loader_node");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");
    WaypointLoader waypoint_loader(nh,pnh);
    ros::spin();
    return 0;
}

3.3 RVIZ可视化演示

运行launch文件

roslaunch waypoint_loader waypoint_loader.launch

打开rviz后，点左下角Add添加path，就能显示出加载出来的路径。

3.4 完整代码下载

【运动规划算法项目实战】如何加载csv文件的路径信息（ROS源码）

---

四、总结

在本文中，我们学习了如何加载CSV文件中的路径信息以在运动规划算法中使用。通过ROS的roscpp库和std::ifstream类，我们可以方便地从文件中读取数据，并将其转换为我们需要的格式。我们还介绍了如何将路径信息转换为ROS消息类型，并将其发布到ROS话题中，以便在其他节点中使用。最后，我们提供了一个完整的代码示例，展示了如何实现路径加载和ROS消息发布的功能。这些技术和示例代码可应用于任何需要加载和使用路径信息的机器人运动规划项目中。