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

最新推荐文章于 2024-08-23 23:27:55 发布

忒懂先生

最新推荐文章于 2024-08-23 23:27:55 发布

阅读量1k

点赞数

分类专栏：运动路径规划MPC算法实战文章标签：机器人自动驾驶人工智能 linux c语言

本文链接：https://blog.csdn.net/weixin_44025389/article/details/132740213

版权

运动路径规划MPC算法实战专栏收录该内容

5 篇文章 0 订阅

订阅专栏

本文提供了一个ROS noetic工程，用于记录和加载机器人运动路径。通过waypoint_saver节点存储路径点到csv文件，waypoint_loader节点读取csv并发布到Rviz。详细讲解了涉及的头文件、cpp文件及其功能，包括路径点的存储、发布以及节点的启动。

摘要由CSDN通过智能技术生成

前言：

承接上一篇博文本文将编写并记录上文中详细的工程项目，用于保存小车的运动路径，生成对应的csv，和加载所保存的路径到实际的Rviz中，本文将开源完整的工程项目，工程结构如下：

工程原码位于文章末尾：

路径存储：

waypoint_saver 用于存储 waypoint 的节点

waypoint_saver.h 头h文件

#ifndef WAYPOINT_SAVER_WAYPOINT_SAVER_H_INCLUDED
#define WAYPOINT_SAVER_WAYPOINT_SAVER_H_INCLUDED

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <fstream>
#include <sstream>

class WaypointSaver
{
public:
    WaypointSaver(ros::NodeHandle nh,ros::NodeHandle pnh);
    ~WaypointSaver();
private:
    void InitializeWaypointsFile_(void);
    void PublishPointsMarkerArray_(geometry_msgs::PoseStamped pose);
    void CurrentPoseCallback_(const geometry_msgs::PoseStamped::ConstPtr msg);

    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;

    std::string wps_marker_topic_;
    std::string current_pose_topic_;
    std::string waypoints_path_;
    std::string map_frame_;
    double wp_interval_;

    std::ofstream ofs_;
    bool init_pose_received_;
    geometry_msgs::PoseStamped previous_pose_;

    
    ros::Subscriber current_pose_sub_;
    ros::Publisher wps_marker_pub_;
};

#endif  //WAYPOINT_SAVER_WAYPOINT_SAVER_H_INCLUDED

解释:

头文件：定义了一个名为 WaypointSaver 的类，用于保存路径点。该类使用了ROS（Robot Operating System）库，并包含了必要的头文件。它具有一些私有成员变量和函数，其中 InitializeWaypointsFile_() 用于初始化路径点文件，PublishPointsMarkerArray_() 用于发布路径点视觉化信息，CurrentPoseCallback_() 是当前姿态的回调函数。该类的构造函数和析构函数分别被定义为公有成员，接受两个 ros::NodeHandle 对象作为参数。

waypoint_saver.cpp路径存储

#include <waypoint_saver/waypoint_saver.h>

WaypointSaver::WaypointSaver(ros::NodeHandle nh,ros::NodeHandle pnh) : nh_(nh),pnh_(pnh)
{
    pnh_.param<std::string>("current_pose_topic", current_pose_topic_, "current_pose");
    pnh_.param<std::string>("wps_marker_topic", wps_marker_topic_, "waypoint_saver_marker");
    pnh_.param<std::string>("waypoints_csv", waypoints_path_, "/tmp/saved_waypoints.csv");
    pnh_.param<std::string>("map_frame", map_frame_, "map");
    pnh_.param<double>("wp_interval", wp_interval_, 1.0);
    wps_marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(wps_marker_topic_, 1);

    init_pose_received_ = false;
    WaypointSaver::InitializeWaypointsFile_();

    current_pose_sub_ = nh_.subscribe(current_pose_topic_, 10, &WaypointSaver::CurrentPoseCallback_, this);
}

WaypointSaver::~WaypointSaver()
{
    ofs_.close();
}

void WaypointSaver::InitializeWaypointsFile_(void)
{
    ofs_.open(waypoints_path_, std::ios::app);
    ofs_ << "x,y,z,yaw,velocity,mission" << std::endl;
}

void WaypointSaver::PublishPointsMarkerArray_(geometry_msgs::PoseStamped pose)
{
    static visualization_msgs::MarkerArray marray;
    static int id = 0;

    visualization_msgs::Marker marker;
    marker.id = id;
    marker.header.frame_id = map_frame_;
    marker.header.stamp = ros::Time();
    marker.frame_locked = true;

    marker.ns = "saved_waypoints_arrow";
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;
    marker.scale.x = 0.3;
    marker.scale.y = 0.1;
    marker.scale.z = 0.2;
    marker.color.a = 1.0;
    marker.color.r = 0.0;
    marker.color.g = 1.0;
    marker.color.b = 0.0;
    marker.pose = pose.pose;
    marray.markers.push_back(marker);

    wps_marker_pub_.publish(marray);
    id++;
}

void WaypointSaver::CurrentPoseCallback_(const geometry_msgs::PoseStamped::ConstPtr msg)
{
    geometry_msgs::PoseStamped current_pose = *msg;

    if(!init_pose_received_)
    {
        ofs_ << std::fixed << std::setprecision(4) << current_pose.pose.position.x << "," << current_pose.pose.position.y << ","
             << current_pose.pose.position.z << "," << tf::getYaw(current_pose.pose.orientation) << ",0,0" << std::endl;
        PublishPointsMarkerArray_(current_pose);
        previous_pose_ = current_pose;
        init_pose_received_ = true;
    }
    else
    {
        double distance = sqrt(pow((current_pose.pose.position.x - previous_pose_.pose.position.x), 2) + pow((current_pose.pose.position.y - previous_pose_.pose.position.y), 2));

        if(distance > wp_interval_)
        {
            ofs_ << std::fixed << std::setprecision(4) << current_pose.pose.position.x << "," << current_pose.pose.position.y << ","
                 << current_pose.pose.position.z << "," << tf::getYaw(current_pose.pose.orientation) << ",0,0" << std::endl;
            PublishPointsMarkerArray_(current_pose);
            previous_pose_ = current_pose;
        }
    }
}

解释：

当程序运行时，首先会包含 <waypoint_saver/waypoint_saver.h> 头文件。

然后，创建一个 WaypointSaver 对象，调用其构造函数 WaypointSaver::WaypointSaver(ros::NodeHandle nh,ros::NodeHandle pnh)。构造函数会初始化成员变量，并从 ros::NodeHandle pnh 获取一些设置信息，例如当前姿态的话题名称 current_pose_topic、路径点标记的话题名称 wps_marker_topic、保存路径点的文件路径 waypoints_path、地图坐标系的名称 map_frame，以及路径点之间的间隔 wp_interval。构造函数还会创建一个 ros::Publisher 对象以用于发布路径点标记，并为当前姿态创建一个 ros::Subscriber 对象以接收当前姿态的信息。

接下来，调用构造函数中的 InitializeWaypointsFile_() 函数来初始化路径点文件。该函数会打开文件，并写入文件头。

然后，程序进入主循环，等待接收当前姿态的信息。一旦收到当前姿态的信息，将触发回调函数 CurrentPoseCallback_()。

回调函数 CurrentPoseCallback_() 首先判断是否收到初始姿态，如果没有收到，则将当前姿态写入路径点文件，并发布相应的路径点标记。之后，每次接收到新的姿态时，函数计算当前姿态与上一个姿态之间的距离，并将距离大于设定的间隔的姿态保存到路径点文件，并发布相应的标记。

最后，当程序结束时，会调用 WaypointSaver 对象的析构函数 WaypointSaver::~WaypointSaver()，它会关闭路径点文件的输出流。

总结起来，这段代码的工作流程是创建一个 WaypointSaver 对象并初始化一些设置，然后等待接收当前姿态的信息。每次接收到姿态信息时，判断是否为初始姿态，如果是，则将其保存到路径点文件并发布标记；如果是新姿态，则计算与上一个姿态的距离，如果距离大于设定的间隔，则将其保存到路径点文件并发布标记。最后，程序结束时关闭路径点文件的输出流。

waypoint_saver_node.cpp node启动节点

用于装载参数传参：

#include <waypoint_saver/waypoint_saver.h>

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

解释：

这是 waypoint_saver_node 的主程序代码。

在包含 <waypoint_saver/waypoint_saver.h> 头文件后，通过 ros::init(argc, argv, "waypoint_saver_node") 初始化 ROS 节点，其中 argc 和 argv 是命令行参数，“waypoint_saver_node” 是节点的名称。

然后，创建一个 ros::NodeHandle 对象 nh 和 ros::NodeHandle 对象 pnh。ros::NodeHandle 用于与 ROS 系统通信。

接下来，通过创建 WaypointSaver 对象 waypoint_saver，并传入 nh 和 pnh 作为参数，实例化 WaypointSaver 类。这将调用 WaypointSaver 的构造函数来初始化对象。

然后，使用 ros::spin() 进入 ROS 主循环，等待接收来自 ROS 系统的消息和调用回调函数。

最后，当 ROS 节点关闭或程序结束时，返回 0 以表示正常退出。

路径装载与发布：

waypoint_loader.h

#ifndef WAYPOINT_LOADER_WAYPOINT_LOADER_H_INCLUDED
#define WAYPOINT_LOADER_WAYPOINT_LOADER_H_INCLUDED

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

#include <fstream>
#include <sstream>

//headers in Boost
#include <boost/thread.hpp>
#include <boost/bind.hpp>

class WaypointLoader
{
public:
    WaypointLoader(ros::NodeHandle nh,ros::NodeHandle pnh);
    ~WaypointLoader();
private:
    void PublishWaypoints_(void);
    void LoadWaypointsArray_(void);
    void LoadWaypoint_(const std::string& line, geometry_msgs::PoseStamped* wp);

    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;
    ros::Time current_time_;
    
    std::string path_topic_;
    std::string waypoints_path_;
    std::string map_frame_;
    nav_msgs::Path wps_;
    
    ros::Publisher wps_pub_;
};

#endif  //WAYPOINT_LOADER_WAYPOINT_LOADER_H_INCLUDED

解释：

这段代码是 WaypointLoader 类的头文件。

首先，使用预处理指令 `#ifndef`、`#define` 和 `#endif` 来确保头文件在编译时只被包含一次，以防止重复定义。

然后，包含了一些必要的头文件，包括 ROS 相关的头文件和用于读写文件的头文件。

接下来，定义了 WaypointLoader 类。它有一个公有构造函数 `WaypointLoader(ros::NodeHandle nh,ros::NodeHandle pnh)` 和一个私有析构函数 `~WaypointLoader()`。

私有成员函数 `PublishWaypoints_()` 用于发布路径点信息。它会创建一个 `ros::Publisher` 对象，以发布路径点信息。

私有成员函数 `LoadWaypointsArray_()` 用于加载路径点数组。它会读取路径点文件，并将读取的路径点存储到 `nav_msgs::Path` 类型的变量 `wps_` 中。

私有成员函数 `LoadWaypoint_(const std::string& line, geometry_msgs::PoseStamped* wp)` 是加载单个路径点的辅助函数。它将一个字符串解析为路径点信息，并存储在 `geometry_msgs::PoseStamped` 类型的变量 `wp` 中。

接下来，定义了一些私有成员变量。它们包括 ROS 的 `NodeHandle` 对象 `nh_` 和 `pnh_`，用于与 ROS 系统通信；当前时间 `current_time_`；路径点话题名称 `path_topic_`；路径点文件路径 `waypoints_path_`；地图坐标系的名称 `map_frame_`；存储路径点信息的 `nav_msgs::Path` 类型变量 `wps_`；路径点发布者 `wps_pub_`。

最后，使用 `#endif` 结束条件编译指令，确保头文件的完整性。

waypoint_loader.cpp

#include <waypoint_loader/waypoint_loader.h>

WaypointLoader::WaypointLoader(ros::NodeHandle nh,ros::NodeHandle pnh) : nh_(nh),pnh_(pnh)
{
    pnh_.param<std::string>("path_topic", path_topic_, "/waypoints_raw");
    pnh_.param<std::string>("waypoints_csv", waypoints_path_, "/tmp/waypoints.csv");
    pnh_.param<std::string>("map_frame", map_frame_, "odom");
    wps_pub_ = nh_.advertise<nav_msgs::Path>(path_topic_, 10);
    WaypointLoader::LoadWaypointsArray_();
    boost::thread wp_publish_thread(boost::bind(&WaypointLoader::PublishWaypoints_, this));
}

WaypointLoader::~WaypointLoader()
{

}

void WaypointLoader::PublishWaypoints_(void)
{
    ros::Rate loop_rate(10);
    while(ros::ok())
    {
        wps_pub_.publish(wps_);
        loop_rate.sleep();
    }
    return;
}

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

    std::ifstream ifs(waypoints_path_);

    if(!ifs)
    {
        ROS_ERROR("Error! File can not be opened");
        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::vector<std::string> columns;
    std::string column;
    std::istringstream stream(line);

    while (getline(stream, column, ','))
    {
        columns.push_back(column);
    }

    wp->pose.position.x = std::stod(columns[0]);
    wp->pose.position.y = std::stod(columns[1]);
    wp->pose.position.z = std::stod(columns[2]);

    geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(std::stod(columns[3]));
    wp->pose.orientation.x = quat.x;
    wp->pose.orientation.y = quat.y;
    wp->pose.orientation.z = quat.z;
    wp->pose.orientation.w = quat.w;

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

解释：

这是 WaypointLoader 类的实现文件。

首先包含了 `waypoint_loader/waypoint_loader.h` 头文件。

接着是 `WaypointLoader` 构造函数的实现。构造函数采用了成员初始化列表，初始化了 `nh_` 和 `pnh_` 成员变量，并根据 `pnh_` 中的参数获取路径点话题名称、路径点文件路径和地图坐标系的名称。然后，创建一个 `ros::Publisher` 对象 `wps_pub_`，用于发布路径点信息。接下来调用 `LoadWaypointsArray_()` 函数来加载路径点数组。最后，创建了一个新线程 `wp_publish_thread`，调用 `PublishWaypoints_()` 函数来周期性地发布路径点信息。

析构函数 `~WaypointLoader()` 是空的，没有实现任何操作。

私有成员函数 `PublishWaypoints_()` 用于周期性地发布路径点信息。在一个循环中，它使用 `wps_pub_` 发布 `wps_` 变量中存储的路径点信息，然后调用 `loop_rate.sleep()` 使线程休眠一段时间，控制发布频率。

私有成员函数 `LoadWaypointsArray_()` 用于加载路径点数组。在函数内部，首先获取当前时间并将其赋值给路径点数组变量 `wps_` 的时间戳和帧 ID。然后，打开路径点文件，如果文件无法打开，则输出错误信息并返回。接下来，读取文件中的每一行，调用 `LoadWaypoint_()` 函数将行解析为一个路径点，并将其添加到 `wps_.poses` 中。

私有成员函数 `LoadWaypoint_(const std::string& line, geometry_msgs::PoseStamped* wp)` 用于将一行文本解析为一个路径点。函数首先将行分割为多个列，并将这些列存储到 `columns` 向量中。然后，根据列的值将位置和姿态信息存储到 `wp` 变量的 `pose` 成员中。最后，为 `wp` 设置时间戳和帧 ID。

总结起来，这段代码实现了 WaypointLoader 类的构造函数、析构函数和一些辅助函数。构造函数用于进行初始化操作，包括参数获取、发布器创建和路径点加载；析构函数为空；`PublishWaypoints_()` 函数用于定期发布路径点信息；`LoadWaypointsArray_()` 函数用于加载路径点数组；`LoadWaypoint_()` 函数用于解析单个路径点的信息。

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;
}

解读：

这是 `waypoint_loader_node` 的主程序代码。

在包含 `<waypoint_loader/waypoint_loader.h>` 头文件后，通过 `ros::init(argc, argv, "waypoint_loader_node")` 初始化 ROS 节点，其中 `argc` 和 `argv` 是命令行参数，"waypoint_loader_node" 是节点的名称。

然后，创建一个 `ros::NodeHandle` 对象 `nh` 和 `ros::NodeHandle` 对象 `pnh`。`ros::NodeHandle` 用于与 ROS 系统通信。

接下来，通过创建 `WaypointLoader` 对象 `waypoint_loader`，并传入 `nh` 和 `pnh` 作为参数，实例化 WaypointLoader 类。这将调用 WaypointLoader 的构造函数来初始化对象。

然后，使用 `ros::spin()` 进入 ROS 主循环，等待接收来自 ROS 系统的消息和调用回调函数。

最后，当 ROS 节点关闭或程序结束时，返回 0 以表示正常退出。

总结：

以上代码是一个ROS节点程序，用于加载路径点并发布到指定的ROS话题。

头文件 "waypoint_loader/waypoint_loader.h" 中定义了 WaypointLoader 类的声明。这个类负责加载路径点并发布到ROS话题。

构造函数 `WaypointLoader::WaypointLoader(ros::NodeHandle nh,ros::NodeHandle pnh)` 接收两个参数，分别是ROS的全局节点句柄 `nh` 和私有节点句柄 `pnh`。在构造函数中，通过 `pnh_` 从ROS参数服务器中获取了路径点话题名称、路径点文件路径和地图坐标系的名称。然后，创建一个发布器 `wps_pub_`，类型为 `ros::Publisher`，用于发布路径点信息。接下来调用 `LoadWaypointsArray_()` 函数来加载路径点数组。最后，创建一个新线程 `wp_publish_thread`，调用 `PublishWaypoints_()` 函数来周期性地发布路径点信息。

`WaypointLoader::~WaypointLoader()` 是析构函数，为空，没有实现任何操作。

`void WaypointLoader::PublishWaypoints_(void)` 是一个私有成员函数，用于周期性地发布路径点信息。在一个循环中，它使用发布器 `wps_pub_` 发布存储在变量 `wps_` 中的路径点信息，然后调用 `loop_rate.sleep()` 使线程休眠一段时间，控制发布频率。

`void WaypointLoader::LoadWaypointsArray_(void)` 是一个私有成员函数，用于加载路径点数组。首先，获取当前时间并将其赋值给路径点数组变量 `wps_` 的时间戳和帧ID。然后，打开定义的路径点文件，如果文件无法打开，报错并返回。接下来，读取文件的每一行，调用 `LoadWaypoint_()` 函数来将行解析为一个路径点，并将其添加到变量 `wps_.poses` 中。

`void WaypointLoader::LoadWaypoint_(const std::string& line, geometry_msgs::PoseStamped* wp)` 是一个私有成员函数，用于将一行文本解析为一个路径点。首先，它将行分割为多个列，并将这些列存储到 `columns` 向量中。然后，根据这些列的值将位置和姿态信息存储到变量 `wp` 的 `pose` 成员中。最后，为 `wp` 设置时间戳和帧ID。

最后，`main()` 函数是程序的入口点。它首先通过命令行参数初始化ROS节点，并创建全局节点句柄 `nh` 和私有节点句柄 `pnh`。然后，实例化 WaypointLoader 类，传入两个节点句柄作为参数，来创建 WaypointLoader 对象。接着，通过调用 `ros::spin()` 进入ROS的主循环，等待接收ROS系统的消息和调用回调函数。当ROS节点关闭或程序结束时，返回 0 以表示正常退出。