Rviz——Marker

一个立方体模拟目标，添加文字描述位置、速度，目标航向的例子；

void SubscribeAndPublish::pub_radarlist_func(){
    int R_col=R.cols();
    visualization_msgs::MarkerArray A;
    A.markers.resize(R_col);//marker矩阵的大小
    
    visualization_msgs::MarkerArray A_Text;
    A_Text.markers.resize(R_col);//marker矩阵的大小

    visualization_msgs::MarkerArray A_Arrow;
    A_Arrow.markers.resize(R_col);//marker矩阵的大小

    if(R_col){
        for(int i=0;i<R_col;i++){
        //添加图形数组
            A.markers[i].header.frame_id = "/my_frame";
            A.markers[i].header.stamp = ros::Time::now();
            // 为这个形状设置命名空间及id 从而创建独一无二的目标 若出现新的目标，其与旧目标有相同的命名空间及id，那么新目标会覆盖旧的目标；  
            A.markers[i].ns = "RadarArray_shapes";
            A.markers[i].id = i;
            A.markers[i].type = visualization_msgs::Marker::CUBE;
            A.markers[i].action = visualization_msgs::Marker::ADD;
            A.markers[i].pose.position.x = R(0,i);
            A.markers[i].pose.position.y = R(1,i);
            A.markers[i].pose.position.z = 0;
            A.markers[i].pose.orientation.x = 0.0;
            A.markers[i].pose.orientation.y = 0.0;
            A.markers[i].pose.orientation.z = 0.0;
            A.markers[i].pose.orientation.w = 1.0;
            A.markers[i].scale.x = 2;
            A.markers[i].scale.y = 2;
            A.markers[i].scale.z = 2;
            A.markers[i].color.r = 255;
            A.markers[i].color.g = 255;
            A.markers[i].color.b = 0;
            A.markers[i].color.a = 1.0;
            //marker.lifetime = ros::Duration();
            // 永久存在  表示存在多久，ros::Duration();意味着永不删除，但是新的具有相同的ns和id的目标将会覆盖旧的，不想被覆盖（想留下轨迹）的话，可以将id+1；否则，就会显示每次的最新值
            A.markers[i].lifetime = ros::Duration(0.1);//存在0.1s
      //添加文字数组
            A_Text.markers[i].header.frame_id = "/my_frame";
            A_Text.markers[i].header.stamp = ros::Time::now();
            // 为这个形状设置命名空间及id 从而创建独一无二的目标 若出现新的目标，其与旧目标有相同的命名空间及id，那么新目标会覆盖旧的目标；  
            A_Text.markers[i].ns = "RadarArray_shapes_Text";
            A_Text.markers[i].id = i;
            A_Text.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
            A_Text.markers[i].action = visualization_msgs::Marker::ADD;
            A_Text.markers[i].pose.position.x = R(0,i);
            A_Text.markers[i].pose.position.y = R(1,i)-13;
            A_Text.markers[i].pose.position.z = 0;
            A_Text.markers[i].scale.x = 2;
            A_Text.markers[i].scale.y = 2;
            A_Text.markers[i].scale.z = 2;
            A_Text.markers[i].color.r = 255;
            A_Text.markers[i].color.g = 255;
            A_Text.markers[i].color.b = 0;
            A_Text.markers[i].color.a = 1.0;
            A_Text.markers[i].lifetime = ros::Duration(0.1);
            if(R(2,i)<0.0001) R(2,i)=0.0;
            if(R(3,i)<0.0001) R(3,i)=0.0;//只是在显示的时候，将其精度降低
            ostringstream str;
            str<<"x,y = "<< R(0,i) <<","<< R(1,i)<<endl
               <<"vx,vy = "<< R(2,i) <<","<< R(3,i)<<endl;
            A_Text.markers[i].text=str.str();
        //添加箭头朝向
            A_Arrow.markers[i].header.frame_id = "/my_frame";
            A_Arrow.markers[i].header.stamp = ros::Time::now();
            A_Arrow.markers[i].ns = "RadarArrayARROW_shapes";
            A_Arrow.markers[i].id = i;
            A_Arrow.markers[i].type = visualization_msgs::Marker::ARROW;
            A_Arrow.markers[i].action = visualization_msgs::Marker::ADD;
            A_Arrow.markers[i].scale.x = 0.2;
            A_Arrow.markers[i].scale.y = 0.4;
            A_Arrow.markers[i].scale.z = 0;
            A_Arrow.markers[i].points.resize(2);
            A_Arrow.markers[i].points[0].x=R(0,i);
            A_Arrow.markers[i].points[0].y=R(1,i);
            A_Arrow.markers[i].points[1].x=R(0,i) + 8*Intervel_Time2*R(2,i);
            A_Arrow.markers[i].points[1].y=R(1,i) + 8*Intervel_Time2*R(3,i);
            A_Arrow.markers[i].pose.orientation.x = 0.0;
            A_Arrow.markers[i].pose.orientation.y = 0.0;
            A_Arrow.markers[i].pose.orientation.z = 0.0;
            A_Arrow.markers[i].pose.orientation.w = 1.0;
            A_Arrow.markers[i].color.r = 255;
            A_Arrow.markers[i].color.g = 192;
            A_Arrow.markers[i].color.b = 203;
            A_Arrow.markers[i].color.a = 1.0;
            A_Arrow.markers[i].lifetime = ros::Duration(0.1);
        }
        //后续应该查看是否要使用Duration()，因为一个目标如果消失了，理论上是应该将其删除的，而不是永久保存在画面上；
        //发布Marker
        radar_marker_pub.publish(A); 
        radar_marker_pub.publish(A_Text); 
        radar_marker_pub.publish(A_Arrow);         

    }
}