yaml参数读取

Hazelxuz

于 2024-05-04 16:06:20 发布

阅读量238

点赞数 9

文章标签：无人机

本文链接：https://blog.csdn.net/YEyeyee_/article/details/138443704

版权

#include <ros/ros.h>
#include <vector>
#include <iostream>

struct Point {
  float x, y, z;
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "read_points");
    ros::NodeHandle nh("~");

    // 读取参数服务器上的points参数
    std::vector<Point> points;
    XmlRpc::XmlRpcValue points_list;
    
    if (nh.getParam("points", points_list)) {
        ROS_ASSERT(points_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
        
        for (int i = 0; i < points_list.size(); ++i) {
            ROS_ASSERT(points_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
            ROS_ASSERT(points_list[i].hasMember("x") && points_list[i].hasMember("y") && points_list[i].hasMember("z"));
            Point p;
            p.x = static_cast<double>(points_list[i]["x"]);
            p.y = static_cast<double>(points_list[i]["y"]);
            p.z = static_cast<double>(points_list[i]["z"]);
            points.push_back(p);
        }
    } else {
        ROS_ERROR("Unable to get param 'points'");
    }

    for (const auto& point : points) {
        std::cout << "Point: x=" << point.x << " y=" << point.y << " z=" << point.z << std::endl;
    }

    ros::spin();
    return 0;
}

yaml：

points:
  - {x: 0.0, y: 0.0, z: 0.5}
  - {x: -0.5, y: -0.5, z: 0.5}
  - {x: 0.5, y: -0.5, z: 0.5}
  - {x: 0.5, y: 0.5, z: 0.5}
  - {x: -0.5, y: 0.5, z: 0.5}
  - {x: 0.0, y: 0.0, z: 0.5}

#include <ros/ros.h> #include <vector> #include <iostream>

这部分是包含指令，它们告诉编译器包含ROS的核心头文件、标准模板库中的vector容器和输入输出流库，以便你可以使用ROS功能、向量和标准输入输出。

struct Point { float x, y, z; };

这定义了一个结构体Point，包含三个浮点数成员变量x、y、z。这用于表示三维空间中的一个点。

int main(int argc, char **argv) { ros::init(argc, argv, "read_points"); ros::NodeHandle nh("~");

这部分是程序的主函数。ros::init(argc, argv, "read_points");初始化ROS系统，命名该节点为"read_points"。ros::NodeHandle nh("~");创建一个节点句柄，"~"代表这个句柄是专用于该私有名称空间。

std::vector<Point> points; XmlRpc::XmlRpcValue points_list;

这里声明了一个Point类型的vector，用于存储点集，还有一个XmlRpcValue类型的变量points_list，用于暂存从参数服务器获取的数据。

if (nh.getParam("points", points_list)) {

这行代码尝试从ROS参数服务器获取名为"points"的参数，并将其存储在points_list中。如果成功，执行下面的代码块。

ROS_ASSERT(points_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

ROS_ASSERT是一个调试工具，用于检查points_list是否为一个数组类型(TypeArray)。如果不是，程序将在这里中断。

for (int i = 0; i < points_list.size(); ++i) {

这个循环遍历points_list数组的每个元素。points_list.size()返回数组的大小，即其中包含的元素数量。

ROS_ASSERT(points_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct); ROS_ASSERT(points_list[i].hasMember("x") && points_list[i].hasMember("y") && points_list[i].hasMember("z"));

对于数组中的每个元素，首先检查它是否是一个结构体(TypeStruct)，然后检查它是否包含x、y、z这三个成员。如果任何检查失败，程序将中断。

Point p; p.x = static_cast<double>(points_list[i]["x"]); p.y = static_cast<double>(points_list[i]["y"]); p.z = static_cast<double>(points_list[i]["z"]); points.push_back(p);

在通过所有检查之后，创建一个Point实例p，并将points_list中当前元素的x、y、z成员分别赋值给p的相应成员变量。然后将p添加到points向量中。

} else { ROS_ERROR("Unable to get param 'points'"); }

如果无法从参数服务器获取"points"参数，将输出一个错误消息。

for (const auto& point : points) { std::cout << "Point: x=" << point.x << " y=" << point.y << " z=" << point.z << std::endl; }

这是一个范围for循环，用于遍历points向量中的每个Point对象，并打印出其x、y、z值。

ros::spin(); return 0; }

最后，ros::spin();使节点保持运行状态，等待并处理回调函数，直到你按下Ctrl+C终止。return 0;表示程序正常退出。

使用代码示例：

#include <vector>
#include <iostream>
#include <ros/ros.h>
// 假设 Command 类型和 move_pub 已经在其他地方定义

struct Point {
  float x, y, z;
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "your_node_name");
    ros::NodeHandle nh;

    // 假设points是从YAML文件中读取并解析完成的点的集合
    std::vector<Point> points = /* 从YAML读取的点 */;

    // 确保至少有一个点
    if (points.empty()) {
        std::cerr << "No points loaded!" << std::endl;
        return -1;
    }

    // 使用points中的第一个点
    Point firstPoint = points[0];

    ros::Publisher move_pub = nh.advertise<YourCommandType>("your_topic_name", 10);
    YourCommandType Command_now; // 假设这是你的消息类型
    ros::Rate rate(20.0); // 20Hz的控制频率

    int sleep_time = 10; // 假设的持续时间
    int i = 0;
    int comid = 0; // 假设的命令ID起始值
    while (i < sleep_time)
    {
        Command_now.command = Move_ENU;
        Command_now.sub_mode = 0;
        Command_now.pos_sp[0] = firstPoint.x;
        Command_now.pos_sp[1] = firstPoint.y;
        Command_now.pos_sp[2] = firstPoint.z;
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;
        move_pub.publish(Command_now);
        rate.sleep();
        cout << "Moving to Point: " << firstPoint.x << ", " << firstPoint.y << ", " << firstPoint.z << endl;
        i++;
    }

    // 其他代码...

    return 0;
}