机器人操作系统ROS从入门到放弃(八):使用rviz进行可视化

欢迎来到第8讲，预计还有两讲结束ros相关的话题。
Rviz作为ROS的可视化工具拥有非常强大的功能，我也不是全部都用过，略懂皮毛。可视化是我们分析问题，向他人展现成果时最重要的工具。打个比方，我们在第三讲能够发布poseStamped类型的消息了，并且在上一讲我们还制作了一个包含poseStamped类型消息，topic名字为chatter的rosbag. 我们得到了不同timestamp下的机器人的pose，那么我们如果向把机器人的pose画出来，既方便我们自己分析，又方便别人看懂，该怎么办呢？自然要用到rviz了。
ros官方的rviz教程在下面的链接中，大家自然得去熟悉一下
http://wiki.ros.org/rviz/UserGuide
我个人的讲解还是会结合例子讲，而不会讲rviz的UI的每个窗口都是包含什么东西。另外要注意的是我用的例子是我在工作中碰到了问题，使用rviz去解决，可并不是说rviz就只能这么用！
上一讲我们制作了的rosbag(你没看上一讲但是已经了解了rosbag的话，就直接去我的github下载下来那个rosbag吧
https://github.com/zhaozhongch/ros_tutorial
rosbag在pub_sub_test/src/material里)。想把每一时刻机器人的pose给画出来，我们就得使用rosbag里的信息。我们说了，使用rosbag时它自身相当于一个publisher，使用的方法是

rosbag play record_poseStamped.bag

有了publisher，我们自然就需要一个subscriber。虽然我们现在还不知道rviz怎么用，但是我们大概能猜到在rviz中我们需要定义一个subsriber，来接收那个rosbag发布出的消息。接下来我们打开rviz。
首先跑roscore，之后打开另一个terminal，在里面输入

rosrun rviz rviz

就打开rviz的界面了。

rviz_blank_screenshot.png
如果有些许不一样不要担心，有可能是我的版本太新了。没错，笔者已经悄悄地升级到了ubuntu18.04LTS系统外加最新版本的ros melodic了，程序猿嘛，就是要跟得上时代。
pretend2B.jpg
咳咳以前的代码都是没有问题能用的这点不要担心。
上面那个界面呢你一进去当然是一脸懵逼的，没有关系，我们还是本着有publisher就需要一个subscriber的想法，找一个消息的接收者。接下来的步骤大家先照着做，等结果出来了我们再来慢慢解释各种原由。
1：点击界面左下方的Add
出现下面窗口
choose_mark.png
2：点击里面的Marker，然后选择OK。这时候我们会发现在rivz界面左边的Displays那个大框框中，Grid下出现了一个新的东西，名字就是我们刚刚选择的Marker。
check_marker.png
Marker旁边有一个小三角，是Marker包含的下拉列表，点一下，就会出来和我这张图一样的东西了。在下拉列表里我们看到了一个很熟悉的东西... Topic。此时，这个Marker就已经是一个subscriber了，并且正在试图接收消息，它目前接收的是来自visualization_marker这个topic的消息。下面还有Queue_Size，也是我们以前定义一个subscriber时所熟悉的内容。Namespace可以暂且不管。
那么marker到底是什么呢？和它的名字一样，它就是一个用来标记的东西。如果我们给定marker一个position和orientation（位置和姿态），那么rviz就会在中间画图区域的指定位置生成一个指定方向的marker，这个marker可以是立方体，箭头等我们可以在程序中自行选择。如何给marker一个orientation和position那自然就是我们发布消息，marker接收消息，消息里面包含了marker的位置和姿态。
下面我们就可以试试写程序，把我们之前的rosbag中记录的position和orientation发布给marker。这样rviz就能在画出每一时刻指定的position和orientation看起来什么样。我们写一个接收rosbag里posetamped的程序，并把posestamp转化为marker能接收的消息种类再发布出来，让marker接收。
我们创建一个新的package来学习rviz

cd ~/catkin_ws/src
catkin_create_pkg learn_rviz_tf roscpp rospy std_msgs geometry_msgs visualization_msgs tf
cd ..
catkin_make

创建的package名称叫learn_rviz_tf，因为下一讲我们会结合rviz一起学习tf，所以就干脆在一个package里写程序了。 他的依赖项除了我们之前常用的外多了visualization_msgs，这里面会包含marker类型的消息;多了tf，tf是我们下一讲的内容。
接下来在learn_rviz_tf的src文件夹创建一个pub_marker_msgs.cpp的文件并在里面输入下面的代码。

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "visualization_msgs/Marker.h"

class MarkerPublisher{
public:
    MarkerPublisher(ros::NodeHandle& nh){
        pub_marker_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);//initialize marker publisher
        set_marker_fixed_property();
    };
    void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
        count_++;
        marker_.ns = "my_namespace";
        marker_.id = count_;
        
        marker_.header.stamp = ros::Time();
        marker_.pose = msg->pose;

        pub_marker_.publish(marker_);
    };
    void set_marker_fixed_property(){
        /*decide from which view we can see the marker*/
        marker_.header.frame_id = "my_frame";
        /*set marker type*/
        marker_.type = visualization_msgs::Marker::SPHERE;
        /*decide if the marker will be enlarge*/
        marker_.scale.x = 1;
        marker_.scale.y = 0.1;
        marker_.scale.z = 0.1;
        /*decide the color of the marker*/
        marker_.color.a = 1.0; // Don't forget to set the alpha!
        marker_.color.r = 0.0;
        marker_.color.g = 1.0;
        marker_.color.b = 0.0;
        /*set marker action*/
        marker_.action = visualization_msgs::Marker::ADD;
    };

private:
    ros::Publisher  pub_marker_;
    visualization_msgs::Marker marker_;
    int count_ = 0;
};




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

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

    ros::NodeHandle n;

    MarkerPublisher mp(n);

    ros::Subscriber sub_pose   = n.subscribe("chatter", 100, &MarkerPublisher::PoseCallback, &mp);

    ros::spin();
}

主函数的前面两行大家都很熟悉了。
第三行我们定义了MarkerPublisher的对象mp并把nodehandle传入，MarkerPublisher这个类是我们建立来发布Marker这种类型的消息的。
第四行我们定义了接收chatter这个topic的接收器，由于callback函数定义在了MarkerPublisher这个类里，所以注意一下定义的方式。这个接收器以及接收函数是用来接收我们rosbag里topic名字为chatter，消息类型为posestamped的消息，所以注意topic的名字和callback函数里参数要和rosbag里一一对应起来。
在类中发布消息的方式我们在第五讲讲过，不熟悉可以再去回顾一下。
spin函数检测是否有消息发布。

接下来看MarkerPublisher这个类。
首先我们已经将visualization_msgs::Marker类型的消息marker定义为私有成员了。这样我们不用每次发布消息时都重新定义一个marker并且为它的各种性质重新赋值。
类的构造函数中我们定义了pub_marker的具体内容，它将要发布topic名为visualization_marker，消息类型为visualization_msgs::Marker的消息。首先topic的名字，是必须和rviz中我们设置的marker的topic名字对应(上一张图里我们可以看到 Marker Topic的名字是visualization_marker)。这个消息类型具体怎么赋值呢？我们第三讲讲过查自己需要的消息类型的方法，我们在搜索引擎中输入类似于visualization_msgs, marker, ros这类的东西，就可以看到下面的网页
http://docs.ros.org/melodic/api/visualization_msgs/html/msg/Marker.html
点进去你会看到这个消息包含的成员变量太多了，头都大了，怎么用不是很清楚。还好对于这种稍微复杂类型的消息，ROS一般有例子可寻找，其实你稍微花点儿功夫就可以找到下面的网页
http://wiki.ros.org/rviz/DisplayTypes/Marker
在网页中exmaple_usage里我们可以看到它有示例代码。我上面的代码也是根据它的内容来的。下面回到我的代码，在构造函数的第二行，我们创建了一个叫set_marker_fixed_property()的函数，这个函数我们设置了一些要发布的marker的一些我们不想改变的性质。比如该函数中的

marker_.header.frame_id = "my_frame"

这行的作用是指定我们在rviz中从哪个坐标系去观看我们的marker。其实道理很简单，大家知道机器人一般有很多参考坐标系，什么世界坐标系，相机坐标系或者其他传感器坐标系，世界坐标系一般固定不动，其他的可能随机器人移动。我们上面定义这个my_frame，表示我们希望从my_frame这个坐标系里去观察marker，而之后，可以想象，我们需要在rviz中把世界坐标系的名字设置为my_frame。二者能对应上，我们就能从一个固定不变的坐标系中观察我们的marker了。
接下来是

 marker_.type = visualization_msgs::Marker::SPHERE;

从字面意思不难理解是把我们marker的样子定义为球形。能定义成哪些形状大家可以看上面参考网页中的资料
这个球得有多大呢，得设置它的scale。所以接下来是

marker_.scale.x = 1;
marker_.scale.y = 0.1;
marker_.scale.z = 0.1;

设置marker的scale，很好理解，marker自身在x,y,z方向上的缩放系数。假设我们把marker的样子设置为一个球，x方向scalr为1，其他为0.1，那么我们应该看到的是一个椭球。
接下来是定义marker这个球的颜色

marker_.color.a = 1.0; // Don't forget to set the alpha!
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;

第一个marker_.color.a要设置为1,marker才看得见(真是奇怪的性质hhhh)，后面三个分别是red,green,blue，颜色由0到1由浅变深，不必细说，如上设置的话marker就是绿色。
接下来是

marker_.action = visualization_msgs::Marker::ADD;

这行表示我们接收到marker的信息了之后是增加相应的marker，你也可以定义删除相应的marker。

上面这些性质我并不想每次更新marker的位置时都设置，所以放到一个函数里了。
接下来我们看callback函数void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
函数的第一行是count_++，其中count_是一个私有成员，可以看到count_在第三行赋值给了frame_id，这是什么意思呢？每一个rviz中出现的marker都有自己特定的id和namespace(即我们第二行给marker_ns赋值的内容)，不同的id或者namespace都代表着不同的marker。第三行代码意味着每一次接收函数接收到一条新的消息，marker的id都会改变。那么每一次接收到新的消息都会有一个新的marker诞生。
可以想象，因为每一次接收到消息都会给marker赋值新的来自于posestamp消息的pose，如果这些pose的id都不同，我们就会看到那个椭球形的marker在不断增加，如果id和namespace都相同，那么我们会只看到一个marker,那个marker仿佛在移动。
callback函数第四行给maker自身包含的timestamp赋值。在这个例子里不是很重要。
第五行就是关键了，我们接收到的pose赋值给marker。为什么能这么直接赋值呢？打开我们的第一个网页链接我们可以看到marker是包含了类型为geometry_msgs/Pose的成员pose的。而posetamped类型的消息包含的是pose和header，所以我们可以直接进行pose的赋值，这样也就定义了marker的位置和方向。
接下来我们只要发布marker_这个消息，rviz就应该能接收了。
等一下，貌似忘了什么。刚才说了我们需要把rviz中世界坐标系的名字和marker的frame_id对应起来。这样我们才能从一个不动的上帝视角看marker。具体的做法是你的rviz中，找到Global Frame下面的Fixed Frame，这个就是世界坐标系。目前它的名字叫map，我们点击那个map，把它改成my_frame。现在就万事俱备了。

把cpp文件写到CMakeLists里

add_executable(pub_marker_msgs src/pub_marker_msgs.cpp)
target_link_libraries(pub_marker_msgs ${catkin_LIBRARIES})

编译好后打开一个terminal，source之后输入

rosrun learn_rviz_tf pub_marker_msgs

开始接收来自topicchatter的geometry_msgs::PoseStamped类型的消息。接收到后发布类型为visualization_msgs::Marker，topic为visualization_marker的消息。
之后再打开一个terminal,cd到rosbag的文件夹跑我们的rosbag，发布topic为chatter的消息

rosbag play record_poseStamped.bag

我们就应该能在rviz中看到类似下面的东西。

multi_marker.gif

如果你拖动鼠标滚轮当然可以看到更多的marker生成了(你的rosbag储存有足够多的信息的话)。说好的椭球呢？恩这就是一个椭球...不过拉伸太用力看起来像一根线了哈哈。
大家可以更改我们之前程序中的设置，比如marker的id固定为一个数，看看会发生什么。

总结

我们连rviz的窗口的的作用都没介绍就上例子然后就总结了，真是不按常理出牌，不过我们下一讲还会用到rviz，例子多做几个自然就知道怎么用了。那些窗口有什么用看官网的tutorial就是了。比如rviz窗口最上面的有选项interact, Move Camera, Select等，你如果左键选择Select，再右键点击Select，勾中Selection选项，在Display下面会新出来一个窗口，你如果此时去点击一个marker，新的窗口会显示marker的坐标，这其实我也是才知道哈哈。
还是那句话,rviz功能强大，我们在实际的例子只能讲到一二，其他的在你们的实际工作中结合官网去慢慢体会。
下一讲我们还会用到rviz，同时结合tf讲解，tf是ros用来保持追踪多个不同传感器坐标的主要工具，十分重要。