【可视化】娱乐一下,rviz上让目标转起来

        【Rviz可视化】娱乐一下,rviz上画个3D框这篇文章画3D框的时候我回避了一个重要的东西，那就是标志物的姿态，这也是我们在做3D点云检测框展示的时候肯定要涉及到的一个问题。为了简单起见，我只考虑偏航角(yaw)，而俯仰角(pitch)和翻滚角(roll)保持为0度，也就是只考虑目标绕Z轴旋转的情况。

 为了比较直观的展示标志物的偏转情况，在这里我设置两个箭头，红色箭头的yaw角在[0,PI]之间变化，绿色箭头yaw角在[0,-PI]之间变化，代码如下：

#include "ros/ros.h"
#include "visualization_msgs/Marker.h"
#include <tf/transform_datatypes.h>
#include <visualization_msgs/MarkerArray.h>

class MarkerPublisher{
public:
    MarkerPublisher(ros::NodeHandle& nh){
        pub_marker_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        set_marker_fixed_property();
    };
    void Publish(){
        visualization_msgs::MarkerArray marker_array;
        marker_pos_.header.stamp = ros::Time();
        //orientation
        yaw_ += 0.005;
        if (yaw_ >= M_PI) {
            yaw_ = 0.;
        }
        tf::Quaternion quaternion = tf::createQuaternionFromYaw(yaw_);
        marker_pos_.pose.orientation.x = quaternion.x();
        marker_pos_.pose.orientation.y = quaternion.y();
        marker_pos_.pose.orientation.z = quaternion.z();
        marker_pos_.pose.orientation.w = quaternion.w();
        marker_array.markers.push_back(marker_pos_);

        quaternion = tf::createQuaternionFromYaw(-yaw_);
        marker_neg_.pose.orientation.x = quaternion.x();
        marker_neg_.pose.orientation.y = quaternion.y();
        marker_neg_.pose.orientation.z = quaternion.z();
        marker_neg_.pose.orientation.w = quaternion.w();
        marker_array.markers.push_back(marker_neg_);
        pub_marker_.publish(marker_array);
    };

    void set_marker_fixed_property(){
        marker_pos_.id = 1;
        marker_pos_.header.frame_id = "livox_frame";
        marker_pos_.ns = "my_namespace";
        /*set marker type*/
        marker_pos_.type = visualization_msgs::Marker::ARROW;
        /*decide if the marker will be enlarge*/
        marker_pos_.scale.x = 2.0; //1m
        marker_pos_.scale.y = 0.2;
        marker_pos_.scale.z = 0.2;
        marker_pos_.pose.position.x = 1.0;
        marker_pos_.pose.position.y = 2.0;
        marker_pos_.pose.position.z = 0;

        /*decide the color of the marker*/
        marker_pos_.color.a = 1.0; // Don't forget to set the alpha!
        marker_pos_.color.r = 1.0;
        marker_pos_.color.g = 0.0;
        marker_pos_.color.b = 0.0;
        /*set marker action*/
        marker_pos_.action = visualization_msgs::Marker::ADD;
        marker_pos_.lifetime = ros::Duration(); //(sec,nsec)

        //
        marker_neg_.id = 2;
        /*decide from which view we can see the marker*/
        marker_neg_.header.frame_id = "livox_frame";
        marker_neg_.ns = "my_namespace";
        /*set marker type*/
        //marker_neg_.type = visualization_msgs::Marker::CUBE;
        marker_neg_.type = visualization_msgs::Marker::ARROW;
        /*decide if the marker will be enlarge*/
        marker_neg_.scale.x = 2.0; //1m
        marker_neg_.scale.y = 0.2;
        marker_neg_.scale.z = 0.2;

        marker_neg_.pose.position.x = -1.0;
        marker_neg_.pose.position.y = -2.0;
        marker_neg_.pose.position.z = 0;
        /*decide the color of the marker*/
        marker_neg_.color.a = 1.0; // Don't forget to set the alpha!
        marker_neg_.color.r = 0.0;
        marker_neg_.color.g = 1.0;
        marker_neg_.color.b = 0.0;
        /*set marker action*/
        marker_neg_.action = visualization_msgs::Marker::ADD;
        marker_neg_.lifetime = ros::Duration(); //(sec,nsec)
    };

private:
    ros::Publisher  pub_marker_;
    visualization_msgs::Marker marker_pos_;
    visualization_msgs::Marker marker_neg_;
    double yaw_ = 0;
};

int main(int argc, char **argv){
    ros::init(argc, argv, "marker_worker");
    ros::NodeHandle nh;
    MarkerPublisher mp(nh);
    ros::Rate loop_rate(50);
    while (nh.ok()) {
        mp.Publish();
        loop_rate.sleep();
    }
    return 0;
}

因为是同时显示两个箭头，所以代码中创建了两个Marker，id分别为1和2。箭头1(红色)坐标位置为(1,2,0)，箭头2(绿色)坐标位置为(-1,-2,0)。箭头1绕z轴角度由[0,PI]变化，箭头2绕z轴角度由[0,-PI]变化。 运行后你在rviz上看到的演示画面如下：