ROS-发布三维框体和标签信息

#include "ros/ros.h"
#include "Roslidar16.h"
#include "Roslidar16.cpp"
#include "std_msgs/Int32MultiArray.h"
#include <thread>
#include <condition_variable>
#include <visualization_msgs/MarkerArray.h>
#include <mutex>

ros::Publisher pub;
ros::Publisher pointCloud_pub;
ros::Publisher line_pub;
ros::Publisher text_Pub;
int rand_num = 0;
using namespace lidar_obstacle_detection;

void draw_lines(visualization_msgs::Marker *line_list)
{
    std::mutex mut;
    mut.lock();
    geometry_msgs::Point p1,p2,p3,p4,p5,p6,p7,p8;
    p1.x = p2.x = p3.x = p4.x = p5.x = p6.x = p7.x = p8.x =0;
    p1.y = p2.y = p3.y = p4.y = p5.y = p6.y = p7.y = p8.y =0;
    p1.z = p2.z = p3.z = p4.z = p5.z = p6.z = p7.z = p8.z =0;

    p2.y = p3.x = p3.y =p4.x = rand_num;//底面
    p5.z = p6.z =p7.z =p8.z = p6.y = p7.x = p7.y =p8.x = rand_num;

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


    line_list->points.push_back(p5);
    line_list->points.push_back(p6);

    line_list->points.push_back(p6);
    line_list->points.push_back(p7);

    line_list->points.push_back(p7);
    line_list->points.push_back(p8);

    line_list->points.push_back(p8);
    line_list->points.push_back(p5);

    //高
    line_list->points.push_back(p1);
    line_list->points.push_back(p5);


    line_list->points.push_back(p2);
    line_list->points.push_back(p6);

    line_list->points.push_back(p3);
    line_list->points.push_back(p7);

    line_list->points.push_back(p4);
    line_list->points.push_back(p8);

    //框框2
    p1.x = p2.x = p3.x = p4.x = p5.x = p6.x = p7.x = p8.x =15;
    p1.y = p2.y = p3.y = p4.y = p5.y = p6.y = p7.y = p8.y =15;
    p1.z = p2.z = p3.z = p4.z = p5.z = p6.z = p7.z = p8.z =15;

    p2.y = p3.x = p3.y =p4.x += rand_num;//底面
    p5.z = p6.z =p7.z =p8.z = p6.y = p7.x = p7.y =p8.x += rand_num;



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


    line_list->points.push_back(p5);
    line_list->points.push_back(p6);

    line_list->points.push_back(p6);
    line_list->points.push_back(p7);

    line_list->points.push_back(p7);
    line_list->points.push_back(p8);

    line_list->points.push_back(p8);
    line_list->points.push_back(p5);

    //高
    line_list->points.push_back(p1);
    line_list->points.push_back(p5);


    line_list->points.push_back(p2);
    line_list->points.push_back(p6);

    line_list->points.push_back(p3);
    line_list->points.push_back(p7);

    line_list->points.push_back(p4);
    line_list->points.push_back(p8);
    mut.unlock();
}

void draw_text(visualization_msgs::MarkerArray *texts_marker)
{
    visualization_msgs::Marker text_marker1,text_marker2,text_marker3;
    text_marker1.header.frame_id="basic_reclidar";
    text_marker1.header.stamp = ros::Time::now();
    text_marker1.ns = "basic_shapes";
    text_marker1.action = visualization_msgs::Marker::ADD;
    text_marker1.pose.orientation.w = 1.0;
    text_marker1.id =8;
    text_marker1.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

    text_marker1.scale.z = 2;
    text_marker1.color.b = 1.0;
    text_marker1.color.g = 0;
    text_marker1.color.r = 1.0;
    text_marker1.color.a = 1;

    geometry_msgs::Pose pose;
    pose.position.x =  rand_num;
    pose.position.y =  rand_num;
    pose.position.z = rand_num;
    text_marker1.text = "name:xxx";
    text_marker1.pose = pose;
    texts_marker->markers.push_back(text_marker1);

    //物体2
    text_marker2.header.frame_id = text_marker3.header.frame_id = "basic_reclidar";
    //text_marker2.header.stamp = ros::Time::now();
    //text_marker2.ns = "basic_shapes";
    text_marker2.action = visualization_msgs::Marker::ADD;
    //text_marker2.pose.orientation.w = 1.0;
    text_marker2.id =9;
    text_marker2.type = text_marker3.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

    text_marker2.scale.z = text_marker3.scale.z = 2.5;
    text_marker2.color.b = text_marker3.color.b = 1.0;
    text_marker2.color.g = text_marker3.color.g = 1.0;
    text_marker2.color.r = text_marker3.color.r = 0;
    text_marker2.color.a = text_marker3.color.a = 1;

    geometry_msgs::Pose pose2;
    pose2.position.x =  18+rand_num;
    pose2.position.y =  18+rand_num;
    pose2.position.z = 18+rand_num;
    text_marker2.text= "name:xxx";
    text_marker2.pose = pose2;
    texts_marker->markers.push_back(text_marker2);

    geometry_msgs::Pose pose3;
    pose3.position.x =  15+rand_num;
    pose3.position.y =  15+rand_num;
    pose3.position.z = 15+rand_num;
    text_marker3.text= "Age:12";
    text_marker3.pose = pose3;
    texts_marker->markers.push_back(text_marker3);
}


void drawCallback () {
    visualization_msgs::Marker line_marker;
    float f = 0.0;
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = "basic_reclidar";
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.id = 5;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.1;
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;

    visualization_msgs::MarkerArray texts_marker;
    rand_num = rand()%10;
    std::thread th_line(draw_lines,&line_list);
    std::thread th_text(draw_text,&texts_marker);
    th_line.join();
    th_text.join();

    line_pub.publish(line_list);
    text_Pub.publish(texts_marker);
}

int main (int argc, char** argv) {
    ros::init (argc, argv, "basic_reclidar");
    ros::NodeHandle nh;
    line_pub = nh.advertise<visualization_msgs::Marker>("line_marker", 10);
    text_Pub = nh.advertise<visualization_msgs::MarkerArray>("text_marker", 10);
    drawCallback();
    ros::spin ();
}