【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上看到的演示画面如下: