ROS笔记之visualization_msgs-Marker学习

在这里插入图片描述

code review!

文章目录

ROS笔记之visualization_msgs-Marker学习
- 一.line_strip例程
- 二.line_list例程一
- 二.line_list例程二
- 二.TEXT_VIEW_FACING例程
- 三.附CMakeLists.txt和package.xml
- 五.关于odom、base_link和map坐标系
- 六.关于visualization_msgs/Marker 的 frame_locked
- 七.visualization_msgs/Marker 的 lifetime
- 八.visualization_msgs/Marker 的action
- 九.visualization_msgs/Marker消息的一些重要字段
- 十.visualization_msgs/Marker 的 Line Strip 和 Line List
- 十一.visualization_msgs::MarkerArray详解
- 十一.visualization_msgs::MarkerArray例程一
- 十二.visualization_msgs::MarkerArray例程二

一.line_strip例程

运行
在这里插入图片描述

main.cc

#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "marker_publisher");
    ros::NodeHandle nh;
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    // 创建一个Line Strip
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "base_link";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "line_strip";
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_strip.pose.orientation.w = 1.0;
    line_strip.scale.x = 0.1; // 线宽度
    line_strip.color.r = 1.0; // 线颜色为红色
    line_strip.color.a = 1.0; // 不透明度为1.0

    // 添加线段的点
    geometry_msgs::Point point1;
    point1.x = 0.0;
    point1.y = 0.0;
    point1.z = 0.0;
    line_strip.points.push_back(point1);

    geometry_msgs::Point point2;
    point2.x = 1.0;
    point2.y = 1.0;
    point2.z = 0.0;
    line_strip.points.push_back(point2);

    geometry_msgs::Point point3;
    point3.x = 2.0;
    point3.y = -1.0;
    point3.z = 0.0;
    line_strip.points.push_back(point3);

    while (ros::ok()) {

        // 发布Line List消息
        marker_pub.publish(line_strip);

        ros::spinOnce();
    }

    return 0;
}

二.line_list例程一

运行
在这里插入图片描述

main.cc

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

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

    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    // 创建Line List消息
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = "map";  // 设置坐标系
    line_list.header.stamp = ros::Time::now();
    line_list.ns = "line_list";
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.1;  // 设置线段宽度
    line_list.color.r = 0.0;
    line_list.color.g = 1.0;
    line_list.color.b = 0.0;
    line_list.color.a = 1.0;

    // 添加线段1的顶点
    geometry_msgs::Point p1, p2;
    p1.x = 0.0;
    p1.y = 0.0;
    p1.z = 0.0;
    p2.x = 1.0;
    p2.y = 0.0;
    p2.z = 0.0;

    // 添加线段2的顶点
    geometry_msgs::Point p3, p4;
    p3.x = 1.0;
    p3.y = 1.0;
    p3.z = 0.0;
    p4.x = 0.0;
    p4.y = 1.0;
    p4.z = 0.0;

    // 添加线段1的两个顶点
    line_list.points.push_back(p1);
    line_list.points.push_back(p2);

    // 添加线段2的两个顶点
    line_list.points.push_back(p3);
    line_list.points.push_back(p4);

    ros::Rate rate(10);  // 发布频率为10Hz

    while (ros::ok()) {
        // 发布Line List消息
        marker_pub.publish(line_list);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

二.line_list例程二

运行
在这里插入图片描述

main.cc

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

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

    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    // 创建Line List消息
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = "map";  // 设置坐标系
    line_list.header.stamp = ros::Time::now();
    line_list.ns = "line_list";
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.1;  // 设置线段宽度
    line_list.color.r = 0.0;
    line_list.color.g = 1.0;
    line_list.color.b = 0.0;
    line_list.color.a = 1.0;

    // 添加线段的顶点
    geometry_msgs::Point p1, p2, p3, p4;
    p1.x = 0.0;
    p1.y = 0.0;
    p1.z = 0.0;
    p2.x = 1.0;
    p2.y = 0.0;
    p2.z = 0.0;
    p3.x = 1.0;
    p3.y = 1.0;
    p3.z = 0.0;
    p4.x = 0.0;
    p4.y = 1.0;
    p4.z = 0.0;

    // 添加线段的顶点到Line List消息中
    line_list.points.push_back(p1);
    line_list.points.push_back(p2);
    line_list.points.push_back(p2);
    line_list.points.push_back(p3);
    line_list.points.push_back(p3);
    line_list.points.push_back(p4);
    line_list.points.push_back(p4);
    line_list.points.push_back(p1);

    ros::Rate rate(10);  // 发布频率为10Hz

    while (ros::ok()) {
        // 发布Line List消息
        marker_pub.publish(line_list);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

二.TEXT_VIEW_FACING例程

运行
在这里插入图片描述

main.cc

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

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

    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    // 创建Marker消息
    visualization_msgs::Marker text_marker;
    text_marker.header.frame_id = "map";  // 设置坐标系
    text_marker.header.stamp = ros::Time::now();
    text_marker.ns = "text_marker";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.pose.position.x = 1.0;  // 设置文字位置
    text_marker.pose.position.y = 1.0;
    text_marker.pose.position.z = 0.0;
    text_marker.pose.orientation.w = 1.0;
    text_marker.scale.z = 0.5;  // 设置文字大小
    text_marker.color.r = 0.0;  // 设置颜色为红色
    text_marker.color.g = 1.0;
    text_marker.color.b = 1.0;
    text_marker.color.a = 1.0;
    text_marker.text = "Hello, RViz!";  // 设置要显示的文字内容

    ros::Rate rate(10);  // 发布频率为10Hz

    while (ros::ok()) {
        // 发布Marker消息
        marker_pub.publish(text_marker);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

三.附CMakeLists.txt和package.xml

文件结构
在这里插入图片描述

附CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(ros_templete)  # Replace with your package name

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

catkin_package(
  INCLUDE_DIRS src/inc
  CATKIN_DEPENDS roscpp std_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  src/inc
)

add_executable(visualization_node
  src/cc/main.cc
)

target_link_libraries(visualization_node
  ${catkin_LIBRARIES}
)

install(TARGETS visualization_node
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

附package.xml

<?xml version="1.0"?>
<package format="2">
    <name>ros_templete</name>
    <version>0.0.0</version>
    <description>>ROS package for handling messages</description>

    <maintainer email="user@todo.todo">user</maintainer>

    <license>TODO</license>

    <build_export_depend>message_generation</build_export_depend>
    <exec_depend>message_runtime</exec_depend>
    <build_depend>newrizon_msgs</build_depend>
    <build_export_depend>newrizon_msgs</build_export_depend>
    <exec_depend>newrizon_msgs</exec_depend>

    <buildtool_depend>catkin</buildtool_depend>
    <build_depend>roscpp</build_depend>
    <build_depend>std_msgs</build_depend>
    <!-- 添加其他功能包的依赖项，如果需要的话 -->

    <exec_depend>roscpp</exec_depend>
    <exec_depend>std_msgs</exec_depend>
    <!-- 添加其他功能包的依赖项，如果需要的话 -->
</package>

五.关于odom、base_link和map坐标系

在ROS中，odom、base_link和map是常见的坐标系（frame）用于机器人定位和导航。

odom（odometry）坐标系：odom坐标系是机器人的里程计坐标系，表示机器人相对于起始位置的运动。odom坐标系通常由里程计传感器（如编码器）提供的运动信息计算得出，并且在运动过程中会累积误差。因此，odom坐标系相对于真实世界可能存在一些漂移或误差。在机器人导航中，通常使用odom坐标系作为局部坐标系。
base_link坐标系：base_link坐标系是机器人的本体坐标系（或称为机器人坐标系），它是与机器人自身的物理结构对应的坐标系。base_link坐标系通常位于机器人的底盘或底座的几何中心，用于表示机器人的位置和姿态。它是机器人的本体参考坐标系，与机器人的移动无关。
map坐标系：map坐标系是全局地图坐标系，表示机器人所在的全局环境或地图。map坐标系的原点通常是事先设定好的参考点，可以是某个固定的地标或起始位置。map坐标系提供了一个固定的参考框架，用于定位和导航机器人在全局环境中的位置。

在机器人导航中，通常会使用传感器数据（如激光雷达、视觉传感器等）来感知周围环境，并使用算法（如SLAM）来建立地图并定位机器人。通过将base_link坐标系相对于odom坐标系的运动与地图进行对齐，可以得到机器人在全局环境中的位置估计，即机器人在map坐标系中的位姿。

总结：

odom坐标系是机器人的里程计坐标系，相对于起始位置的运动。
base_link坐标系是机器人的本体坐标系，与机器人的移动无关。
map坐标系是机器人所在的全局地图坐标系，提供了固定的参考框架用于定位和导航机器人在全局环境中的位置。

六.关于visualization_msgs/Marker 的 frame_locked

在RViz中，visualization_msgs/Marker消息的frame_locked字段用于指定Marker是否锁定其坐标系框架。

当frame_locked字段设置为true时，表示Marker的坐标系框架将被锁定，即Marker将始终在其指定的坐标系中显示，无论相机如何移动或旋转。

当frame_locked字段设置为false时，表示Marker的坐标系框架将与相机坐标系相对，即Marker将随着相机的移动和旋转而相应地调整其位置和姿态。

以下是一个示例，展示了如何设置frame_locked字段：

visualization_msgs::Marker marker;
// 设置其他字段
marker.frame_locked = true;  // 锁定Marker的坐标系框架
marker_pub.publish(marker);

在上述示例中，我们创建了一个visualization_msgs::Marker对象，并设置了其他字段。然后，我们将frame_locked字段设置为true，以锁定Marker的坐标系框架。最后，我们使用marker_pub.publish(marker)将Marker发布到visualization_marker话题上。

通过设置适当的frame_locked值，可以控制Marker在RViz中是否锁定其坐标系框架。这可以影响Marker的位置和姿态如何与相机的移动和旋转相对应。

请注意，默认情况下，frame_locked字段的值为false，这意味着Marker的坐标系框架将与相机坐标系相对。如果需要固定Marker在其指定的坐标系中显示，可以将frame_locked字段设置为true。

总结起来，通过设置frame_locked字段，您可以控制Marker是否锁定其坐标系框架，以便在RViz中根据需要调整Marker的位置和姿态。

七.visualization_msgs/Marker 的 lifetime

在RViz中，visualization_msgs/Marker消息的lifetime字段用于指定Marker的显示时间。lifetime字段是一个rospy.Duration类型的值，表示Marker在RViz中显示的持续时间。

当发布一个Marker消息时，RViz将根据lifetime字段的值来确定Marker的显示时间。一旦超过指定的持续时间，RViz将自动将Marker从显示中移除。

以下是一个示例，展示了如何设置lifetime字段：

visualization_msgs::Marker marker;
// 设置其他字段
marker.lifetime = ros::Duration(5.0);  // 设置显示时间为5秒
marker_pub.publish(marker);

在上述示例中，我们创建了一个visualization_msgs::Marker对象，并设置了其他字段。然后，我们使用ros::Duration类创建一个持续时间为5秒的ros::Duration对象，并将其赋值给lifetime字段。最后，我们使用marker_pub.publish(marker)将Marker发布到visualization_marker话题上。

通过设置适当的lifetime值，可以控制Marker在RViz中显示的持续时间。一旦超过指定的时间，RViz将自动将Marker从显示中移除。

请注意，如果将lifetime字段设置为0，表示Marker应该一直保持显示，直到通过发布DELETE操作或DELETEALL操作来明确删除它。这对于需要手动控制Marker的显示和隐藏非常有用。

总结起来，lifetime字段允许您控制在RViz中显示Marker的持续时间，使您能够根据需要定制Marker的显示时间。

八.visualization_msgs/Marker 的action

在ROS中，visualization_msgs/Marker消息的action字段用于指定在RViz中如何处理Marker。action字段的值可以是以下几种之一：

visualization_msgs::Marker::ADD：将Marker添加到可视化工具中。如果指定的id在可视化工具中已经存在，将替换现有的Marker。
visualization_msgs::Marker::DELETE：删除指定id的Marker。如果指定的id在可视化工具中不存在，操作被忽略。
visualization_msgs::Marker::DELETEALL：删除所有属于该命名空间（ns字段）的Marker。

通过使用不同的action值，可以在RViz中动态地添加、更新或删除Marker。

以下是一些示例，展示了如何使用不同的action值操作Marker：

添加一个新的Marker：

visualization_msgs::Marker marker;
// 设置其他字段
marker.action = visualization_msgs::Marker::ADD;
marker_pub.publish(marker);

更新现有的Marker：

visualization_msgs::Marker marker;
// 设置其他字段
marker.action = visualization_msgs::Marker::ADD;  // 使用ADD操作确保更新现有Marker
marker_pub.publish(marker);

删除特定id的Marker：

visualization_msgs::Marker marker;
marker.id = 1;  // 要删除的Marker的id
// 其他字段可以为空，因为删除操作不需要其他信息
marker.action = visualization_msgs::Marker::DELETE;
marker_pub.publish(marker);

删除所有属于特定命名空间的Marker：

visualization_msgs::Marker marker;
marker.ns = "my_namespace";  // 要删除的Marker所属的命名空间
// 其他字段可以为空，因为删除操作不需要其他信息
marker.action = visualization_msgs::Marker::DELETEALL;
marker_pub.publish(marker);

在上述示例中，我们创建了一个visualization_msgs/Marker对象，并设置了其他字段，然后将action字段设置为适当的值。最后，我们使用marker_pub.publish(marker)将Marker发布到visualization_marker话题上，以便在RViz中执行所需的操作。

请注意，当使用DELETE或DELETEALL操作删除Marker时，只需设置id或ns字段即可。其他字段可以为空，因为删除操作不需要这些信息。

通过使用适当的action值，您可以根据需要在RViz中添加、更新或删除Marker，从而实现对可视化对象的动态控制。

九.visualization_msgs/Marker消息的一些重要字段

下面是visualization_msgs/Marker消息的一些重要字段：

header：消息的头部信息，包括frame_id和timestamp等。
ns：命名空间，用于将Marker分组。
id：Marker的唯一标识符，用于标识不同的Marker。
type：Marker的类型，指定要显示的形状。可以使用visualization_ms- gs/Marker消息定义的常量来设置，如visualization_msgs::Marker::S- PHERE表示球体。
action：Marker的操作类型，指定在RViz中如何处理该Marker。常见的- 操作类型包括ADD、DELETE和DELETEALL。
pose：Marker的位姿，指定Marker在三维空间中的位置和方向。
scale：Marker的尺寸，用于控制Marker的大小或线宽等属性。
color：Marker的颜色，可以设置RGBA值来指定颜色和不透明度。
points：用于线条和多边形等形状的点列表。每个点由geometry_msgs/- Point消息表示。
text：用于文本形状的字符串内容。
lifetime：Marker的生命周期，用于控制Marker在RViz中的显示时间。

十.visualization_msgs/Marker 的 Line Strip 和 Line List

在RViz中，visualization_msgs/Marker消息类型可用于可视化各种图形对象，包括线条（Line Strip）和线段列表（Line List）。这些对象用于在3D环境中呈现线条或连接多个线段。

Line Strip（线条）：Line Strip由一系列连接的线段组成，线段之间按照顺序连接。在RViz中，它们通常用于表示路径、轨迹或连续的线条。每个线段都由两个点定义，相邻线段之间共享一个点。
Line List（线段列表）：Line List由多个独立的线段组成，每个线段由两个点定义。在RViz中，它们通常用于表示离散的线段集合，每个线段之间都是独立的。可以使用Line List来可视化多个不相连的线段。

十一.visualization_msgs::MarkerArray详解

visualization_msgs::MarkerArray是ROS中的消息类型，用于在RViz中发布多个Marker的数组。

visualization_msgs::MarkerArray消息由以下字段组成：

markers（std::vector<visualization_msgs::Marker>）：包含多个visualization_msgs::Marker对象的数组。每个Marker对象都描述了一个可视化元素，如点、线、箭头、文本等。

通过使用visualization_msgs::MarkerArray消息，您可以同时发布多个Marker，并在RViz中以数组的形式显示它们。

以下是一个示例，展示了如何创建和使用visualization_msgs::MarkerArray消息：

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>

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

    ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("marker_array_topic", 10);

    visualization_msgs::MarkerArray marker_array;

    // 创建第一个Marker
    visualization_msgs::Marker marker1;
    // 设置marker1的各种字段
    marker_array.markers.push_back(marker1);

    // 创建第二个Marker
    visualization_msgs::Marker marker2;
    // 设置marker2的各种字段
    marker_array.markers.push_back(marker2);

    // 发布MarkerArray
    marker_array_pub.publish(marker_array);

    ros::spin();

    return 0;
}

在上述示例中，我们包含了所需的头文件和ROS节点的初始化。然后，我们创建了一个ros::Publisher对象来发布visualization_msgs::MarkerArray消息。接下来，我们创建了一个visualization_msgs::MarkerArray对象，并向其中添加两个visualization_msgs::Marker对象。每个Marker对象可以设置其字段，例如type、pose、scale等。最后，我们使用marker_array_pub.publish(marker_array)将MarkerArray消息发布到marker_array_topic话题上。

通过发布visualization_msgs::MarkerArray消息，RViz将会显示数组中的每个Marker，并根据各自的参数进行渲染和显示。

总结起来，visualization_msgs::MarkerArray消息允许您在RViz中同时发布和显示多个Marker。您可以通过填充markers字段来创建Marker数组，并通过发布MarkerArray消息将其发送到适当的话题上。

十一.visualization_msgs::MarkerArray例程一

运行
在这里插入图片描述

main.cc

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>

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

    ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("marker_array", 10);

    // 创建MarkerArray消息
    visualization_msgs::MarkerArray marker_array;

    // 创建第一个Marker
    visualization_msgs::Marker marker1;
    marker1.header.frame_id = "map";
    marker1.header.stamp = ros::Time::now();
    marker1.ns = "marker_array";
    marker1.id = 1;
    marker1.type = visualization_msgs::Marker::SPHERE;
    marker1.pose.position.x = 1.0;
    marker1.pose.position.y = 2.0;
    marker1.pose.position.z = 0.0;
    marker1.pose.orientation.w = 1.0;
    marker1.scale.x = 0.5;
    marker1.scale.y = 0.5;
    marker1.scale.z = 0.5;
    marker1.color.r = 1.0;
    marker1.color.g = 0.0;
    marker1.color.b = 0.0;
    marker1.color.a = 1.0;
    marker_array.markers.push_back(marker1);

    // 创建第二个Marker
    visualization_msgs::Marker marker2;
    marker2.header.frame_id = "map";
    marker2.header.stamp = ros::Time::now();
    marker2.ns = "marker_array";
    marker2.id = 2;
    marker2.type = visualization_msgs::Marker::CUBE;
    marker2.pose.position.x = -1.0;
    marker2.pose.position.y = 2.0;
    marker2.pose.position.z = 0.0;
    marker2.pose.orientation.w = 1.0;
    marker2.scale.x = 0.5;
    marker2.scale.y = 0.5;
    marker2.scale.z = 0.5;
    marker2.color.r = 0.0;
    marker2.color.g = 1.0;
    marker2.color.b = 0.0;
    marker2.color.a = 1.0;
    marker_array.markers.push_back(marker2);

    ros::Rate rate(1);  // 发布频率为1Hz

    while (ros::ok()) {
        // 发布MarkerArray消息
        marker_array_pub.publish(marker_array);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

十二.visualization_msgs::MarkerArray例程二

运行
请添加图片描述

main.cc

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>

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

    ros::Publisher marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("marker_array", 10);
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("marker", 10);

    // 创建MarkerArray消息
    visualization_msgs::MarkerArray marker_array;

    // 创建第一个Marker
    visualization_msgs::Marker marker1;
    marker1.header.frame_id = "map";
    marker1.header.stamp = ros::Time::now();
    marker1.lifetime = ros::Duration();  // 设置持久化属性为false
    marker1.ns = "marker1";
    marker1.id = 1;
    marker1.type = visualization_msgs::Marker::SPHERE;
    marker1.pose.position.x = 1.0;
    marker1.pose.position.y = 2.0;
    marker1.pose.position.z = 0.0;
    marker1.pose.orientation.w = 1.0;
    marker1.scale.x = 0.5;
    marker1.scale.y = 0.5;
    marker1.scale.z = 0.5;
    marker1.color.r = 1.0;
    marker1.color.g = 0.0;
    marker1.color.b = 0.0;
    marker1.color.a = 1.0;

    // 创建第二个Marker
    visualization_msgs::Marker marker2;
    marker2.header.frame_id = "map";
    marker2.header.stamp = ros::Time::now();
    marker2.lifetime = ros::Duration();  // 设置持久化属性为false
    marker2.ns = "marker2";
    marker2.id = 2;
    marker2.type = visualization_msgs::Marker::CUBE;
    marker2.pose.position.x = -1.0;
    marker2.pose.position.y = 2.0;
    marker2.pose.position.z = 0.0;
    marker2.pose.orientation.w = 1.0;
    marker2.scale.x = 0.5;
    marker2.scale.y = 0.5;
    marker2.scale.z = 0.5;
    marker2.color.r = 0.0;
    marker2.color.g = 1.0;
    marker2.color.b = 0.0;
    marker2.color.a = 1.0;

    marker_array.markers.clear();
    marker_array.markers.push_back(marker1);
    marker_array.markers.push_back(marker2);

    ros::Rate rate(1);  // 发布频率为1Hz

    while (ros::ok()) {
        // 发布MarkerArray消息
        marker_array_pub.publish(marker_array);

        // 发布单个Marker消息
        marker_pub.publish(marker1);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}