Ros noetic 机器人坐标记录运动路径和发布 实战教程（B）

前言：

         网上记录Path的写入文件看了一下还挺多的，有用yaml作为载体文件，也有用csv文件的路径信息，也有用txt来记录当前生成的路径信息，载体不重要，反正都是记录的方式，本文主要按yaml的方式写入，本文将撰写csv方式的文件写入格式，完成上一篇文章中的留下的存入格式。

         其中两种方式的主要区别在于，加载yaml所需要的时间较长，而txt，csv读本时间短暂，而且csv文件的跨越性很广，轻小简单，cvs中可以写入bag，bag包中可以有很多信息包括坐标。
 

简介：

       在运动规划算法项目中,路径规划是非常重要的一环。 在实际应用中,我们通常需要将预先规划好的路径以某种方式加载到程序中进行后续处理和运动控制。而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)或分号。

举个简单的例子

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 poses : 该路径消息包含的位姿信息的列表,是路径的关键部分。每个位姿信息是一 个
geometry_ _msgs/PoseStamped 类型，它包含以下几个重要的信息:
std_ msgs/Header header :该位姿信息的头部信息，与nav_ msg::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: getine逐行读取文件中的数据。

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

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

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

3.2核心代码

waypoint_ loader.cpp代码

#include 
 
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(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 : :oWaypointLoader :在对象销毁时,如果wp_ publish_ _thread_ 可加入,则等待线程的结束。
●成员函数WaypointLoader: :Publ ishWaypoints_ : 在一个循环中,将路径点消息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 Joader node CPP代码

#include 
 
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 ,就能显示出加载出来的路径。

五、总结

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