ros MarkerArray 使用

1. 描述

记录一下 MarkerArray 的使用

2. 代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <iostream>
#include <math.h>
#include <cmath> 

using namespace std;

ros::Publisher  marker_array_pub;

int main(int argc,char **argv)
{

    ros::init(argc,argv,"bpearl_perception");

    ros::NodeHandle node;

    ROS_INFO("bpearl perception init..."); 

    marker_array_pub = node.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 10);

    ros::Rate r(1);

    visualization_msgs::MarkerArray marker_array;

    while (ros::ok())
    {
        marker_array.markers.clear();
        visualization_msgs::Marker bbox_marker;
        bbox_marker.header.frame_id = "/marker_frame";
        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "basic_shapes";
        bbox_marker.lifetime = ros::Duration();
        bbox_marker.frame_locked = true;
        bbox_marker.type = visualization_msgs::Marker::CUBE;
        bbox_marker.action = visualization_msgs::Marker::ADD;
        
        int marker_id = 0;
        int marker_nums = 3;
        for (size_t i = 0; i < marker_nums; ++i)
        {
            if(marker_id == 0){
                bbox_marker.id = marker_id;
                bbox_marker.color.r = 1.0f;
                bbox_marker.color.g = 0.0f;
                bbox_marker.color.b = 0.0f;
                bbox_marker.color.a = 1.0;
                bbox_marker.pose.position.x = 1.0;
                bbox_marker.pose.position.y = 0;
                bbox_marker.pose.position.z = 0;
                bbox_marker.scale.x = 0.25;
                bbox_marker.scale.y = 0.25;
                bbox_marker.scale.z = 0.25;
            }else if(marker_id == 1){
                bbox_marker.id = marker_id;
                bbox_marker.color.r = 0.0f;
                bbox_marker.color.g = 1.0f;
                bbox_marker.color.b = 0.0f;
                bbox_marker.color.a = 1.0;
                bbox_marker.pose.position.x = 0.0;
                bbox_marker.pose.position.y = 2.0;
                bbox_marker.pose.position.z = 0;
                bbox_marker.scale.x = 0.25;
                bbox_marker.scale.y = 0.25;
                bbox_marker.scale.z = 0.25;
            }else if(marker_id == 2){
                bbox_marker.id = marker_id;
                bbox_marker.color.r = 0.0f;
                bbox_marker.color.g = 0.0f;
                bbox_marker.color.b = 1.0f;
                bbox_marker.color.a = 1.0;
                bbox_marker.pose.position.x = 0.0;
                bbox_marker.pose.position.y = 0.0;
                bbox_marker.pose.position.z = 3.0;
                bbox_marker.scale.x = 0.25;
                bbox_marker.scale.y = 0.25;
                bbox_marker.scale.z = 0.25;
            }
            marker_array.markers.push_back(bbox_marker);
            ++marker_id;
        }

        marker_array_pub.publish(marker_array);

        r.sleep();  
    }

    ros::spin();

    return 0;
}

3. 结果