上海交通大学：《ROS机器人操作系统基础与实战》课程教学资源（实验指导书）04 调试和可视化

实验编号： 实验指导书 实验项目： 调试与可视化 所属课程： ROS机器人操作系统基础与实战 课程代码： 面向专业： 机电专业 课程负责人： 朱笑笑 年月 日

实验编号： 实 验 指 导 书 实验项目： 调试与可视化 所属课程: ROS 机器人操作系统基础与实战 课程代码: 面向专业: 机电专业 课程负责人: 朱笑笑 年 月 日

、 实验目的 让学生熟悉程序ROS中程序调试的方法，程序数据可视化等方法。为将来的 程序开发提供指导。 二、 实验内容 1.调试消息的使用 2. 检查系统的运行状态 3. 绘制数值量 4. 显示图像 5. 保存回放图像 6. Rqt的使用 三、 实验过程或其他示意图 1.调试消息的使用 在简单的C+程序开发的时候，我们通常会使用printf,或者cout等输出程序中我们 感兴趣的一些信息。而OS为我们提供了更为功能强大，使用更方便的方法，输出一个调试 信息。 1.1输出一个调试信息 OS包含了一组函数或者宏定义来让我们输出一个调试信息，或者错误信息或者警告， 亦或者只是一些简单的信息。 尝试在程序的任何位置使用以下命令在程序运行的终端来输出一个调试信息。 ROS INFO("My INFO message."); 使用这些功能或者宏我们可以不添加任何新的头文件，但是我们可以添加一个 ros/console.h如下 #include #include #include 程序将输出 INFO][1356440230.837067170]:My INFO message. 所有的消息输出时都会显示其等级和当前时间戳如上。其中实践戳时冲1970年1月1 日凌晨0点开始的纳秒时间。 类似于printf,可以在消息中打印出变量的值const double val=3.l4; ROS INFO("My INFO message with argument:%f",val ) 定义一个val变量，以浮点类型输出 另外还可以使用cout初始类似的样式。ROS中支持C+STL流。 ROS INFO STREAM( "My INFO stream message with argument:"<val 2

- 2 - 一、 实验目的 让学生熟悉程序ROS中程序调试的方法，程序数据可视化等方法。为将来的 程序开发提供指导。 二、 实验内容 1. 调试消息的使用 2. 检查系统的运行状态 3. 绘制数值量 4. 显示图像 5. 保存回放图像 6. Rqt 的使用 三、 实验过程或其他示意图 1. 调试消息的使用 在简单的 C++程序开发的时候，我们通常会使用 printf，或者 cout 等输出程序中我们 感兴趣的一些信息。而 ROS 为我们提供了更为功能强大，使用更方便的方法，输出一个调试 信息。 1.1 输出一个调试信息 ROS 包含了一组函数或者宏定义来让我们输出一个调试信息，或者错误信息或者警告， 亦或者只是一些简单的信息。 尝试在程序的任何位置使用以下命令在程序运行的终端来输出一个调试信息。 ROS_INFO( "My INFO message." ); 使用这些功能或者宏我们可以不添加任何新的头文件，但是我们可以添加一个 ros/console.h 如下 #include #include #include 程序将输出 [ INFO] [1356440230.837067170]: My INFO message. 所有的消息输出时都会显示其等级和当前时间戳如上。其中实践戳时冲 1970 年 1 月 1 日凌晨 0 点开始的纳秒时间。 类似于 printf，可以在消息中打印出变量的值 const double val = 3.14; ROS_INFO( "My INFO message with argument: %f", val ); 定义一个 val 变量，以浮点类型输出 另外还可以使用cout初始类似的样式。ROS中支持C++STL流。 ROS_INFO_STREAM( "My INFO stream message with argument: " << val );

可以得到和上面一样的效果。注意传统的C+流包含了Cout和cerr,在ROS中并没有显示 的说明使用了哪个流，这个通过对应的调试信息的level来确定。 1.2设置调试信息的1eve1 ROS有5个典型的日志1evel,从低到高为DEBUG,INFO,WARN,ERROR,和FATAL. 在打印消息时可以指定消息的level ROS「J 其中DEBUG和NFO都会到用cout(或者stdout),而WARN,ERROR,和FATAL将会 进到cerr(或者stderr)。同时各种levelt的消息在打印的时候后有不同的颜色以便于区别。 DEBUG为绿色，NFO白色，WARN黄色，ERROR红色，and FATAL紫色 利用调试消息的LEVEL可以让我们的调试信息组织的更加清晰，在实际使用过程中，我 们可以设定一个最低的Level,低于该Levell的消息将会被忽略掉。 在一个实际的node中配置调试1evel 默认情况下是有INFO或者以上level的消息才会显示.ROS使用这个消息level来滤除一 个实际ode发送的消息。有很多实现方法。可以在程序编译的时候，可以在程序启动的时候 通过一个配置文件来实现，除此之外还可以动态的改变这个level,具体来说可以利用 rosconsloerxconsole 首先，通过源码的形式实现在编译过程中确定调试level。在main函数中，在调用了ros: init后，插入以下代码： /Set the logging level manually to DEBUG ROSCONSOLE AUTOINIT; log4cxx::LoggerPtr my_logger log4cxx::Logger:getLogger(ROSCONSOLE DEFAULT_NAME ) my_logger->setLevel( ros::console::g_level_lookup[ros::console::levels::Debug] 注意，在CMakeLists..txt中需要链接到1og4 cxxlibrary。 find library(LOG4CXX LIBRARY log4cxx) add_executable(examplel src/example1.cpp) target link libraries(examplel Sfcatkin LIBRARIES SLOG4CXX LIBRARY}) 可以观察到这样修改后，DEBUG(或更高)level的消息了。请自己修改成其它的级别查 看效果。 测试通过配置文件来改变调试1evel的方法。 首先，创建一个文件夹config。.并创建一个文件debug_level.config,在该文件中输入 以下内容 log4j.logger.ros.chapter4=DEBUG 然后，我们可以通过设置ROSCONSOLE CONFIG FILE环境变量指向这个文件，就 可以起到作用。也可以通过launch文件的方法来实现。在原来的launch方法文件基础上添加 -3-

- 3 - 可以得到和上面一样的效果。注意传统的C++流包含了Cout和cerr，在ROS中并没有显示 的说明使用了哪个流，这个通过对应的调试信息的level来确定。 1.2 设置调试信息的level ROS有5个典型的日志level，从低到高为DEBUG, INFO, WARN, ERROR,和FATAL. 在打印消息时可以指定消息的level ROS_「」 其中DEBUG和INFO都会到用cout（或者stdout）， 而WARN, ERROR,和FATAL将会 进到cerr（或者stderr）。同时各种level的消息在打印的时候后有不同的颜色以便于区别。 DEBUG为绿色，INFO白色, WARN 黄色, ERROR红色, and FATAL紫色 利用调试消息的LEVEL可以让我们的调试信息组织的更加清晰，在实际使用过程中，我 们可以设定一个最低的Level，低于该Level的消息将会被忽略掉。 在一个实际的node中配置调试level 默认情况下是有INFO或者以上level的消息才会显示。ROS 使用这个消息level来滤除一 个实际node发送的消息。有很多实现方法。可以在程序编译的时候，可以在程序启动的时候 通过一个配置文件来实现，除此之外还可以动态的改变这个level，具体来说可以利用 rosconsloe和rxconsole 首先，通过源码的形式实现在编译过程中确定调试level。在main函数中，在调用了ros：： init后，插入以下代码： // Set the logging level manually to DEBUG ROSCONSOLE_AUTOINIT; log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger( ROSCONSOLE_DEFAULT_NAME ); my_logger->setLevel( ros::console::g_level_lookup[ros::console::levels::Debug] ); 注意，在CMakeLists.txt中需要链接到log4cxxlibrary。 find_library(LOG4CXX_LIBRARY log4cxx) add_executable(example1 src/example1.cpp) target_link_libraries(example1 ${catkin_LIBRARIES} ${LOG4CXX_LIBRARY}) 可以观察到这样修改后，DEBUG（或更高）level的消息了。请自己修改成其它的级别查 看效果。 测试通过配置文件来改变调试level的方法。 首先，创建一个文件夹config。并创建一个文件debug_level.config，在该文件中输入 以下内容 log4j.logger.ros.chapter4=DEBUG 然后，我们可以通过设置ROSCONSOLE_ CONFIG_FILE 环境变量指向这个文件，就 可以起到作用。也可以通过launch文件的方法来实现。在原来的launch方法文件基础上添加

env字段。 为消息添加一个名字 因为在同一个ode中的任何地方都可以发送消息消息，这样当我们在多处发出消息时就 不确定时哪个位置发出的消息。为了解决这个问题，ROS提供了一个能够在发出消息时附加 一个名字的方法。具体是使用ROS[STREAM]NAMED功能，请添加以下代码 并观察输出结果 ROS INFO STREAM NAMED( "named msg", "My named INFO stream message;val ="STREAM]COND[NAMED]函数可以实现该功能。 /Conditional messages: ROS INFO STREAM COND( val =0., "My conditional INFO stream message;val ("=0" /Conditional Named messages: ROS INFO STREAM COND NAMED( "cond named msg",val =0., "My conditional INFO stream message;val ("=0" -4-

- 4 - env字段。 为消息添加一个名字 因为在同一个node中的任何地方都可以发送消息消息，这样当我们在多处发出消息时就 不确定时哪个位置发出的消息。为了解决这个问题，ROS提供了一个能够在发出消息时附加 一个名字的方法。具体是使用ROS_[_STREAM]_NAMED 功能，请添加以下代码 并观察输出结果 ROS_INFO_STREAM_NAMED( "named_msg", "My named INFO stream message; val = " [_STREAM]_COND[_NAMED] 函数可以实现该功能。 // Conditional messages: ROS_INFO_STREAM_COND( val = 0., "My conditional INFO stream message; val (" = 0" ); // Conditional Named messages: ROS_INFO_STREAM_COND_NAMED( "cond_named_msg", val = 0., "My conditional INFO stream message; val (" = 0" );

滤除的消息功能本质和带条件的非常类似，但是它允许我们去定义一个从 ros:console:FilterBase扩展出来的自定义的滤波器。使用 ROSSTREAM)FILTER[NAMED]函数即可以使用滤波器的消息，其中第一 个参数为滤波器指针。例子如下： struct MyLowerFilter:public ros::console::FilterBase{ MyLowerFilter(const double&val )value(val ) inline virtual bool isEnabled() return value =0.; double value; ； MyLowerFilter filter_lower(val)；方 MyGreaterEqualFilter filter_greater_equal(val ) ROS INFO STREAM FILTER( &filter lower. "My filter INFO stream message;val ("=0" 其它类型的消息一单次、定时、组合。 测试控制消息的发出次数。利用ROSSTREAM)ONCE[NAMED]函数可以显 示消息的单次输出。无论函数被调用了几次，只有一次输出 for(int i=0;i<10;+i){ ROS_INFO _STREAM_ONCE( "My once INFO stream message;i="<<i ) 定时的输出，可以控制该消息在一定时间内可以输出，过了该时间短，即使再次调用该函数 也不会再有输出。第一个参数为时间区域。 for(int i=0;i<10;+i){ -5-

- 5 - 滤除的消息功能本质和带条件的非常类似，但是它允许我们去定义一个从 ros::console::FilterBase 扩展出来的自定义的滤波器。使用 ROS_[_STREAM]_FILTER[_NAMED] 函数即可以使用滤波器的消息，其中第一 个参数为滤波器指针。例子如下： struct MyLowerFilter : public ros::console::FilterBase { MyLowerFilter( const double& val ) : value( val ) {} inline virtual bool isEnabled() { return value = 0.; } double value; }; MyLowerFilter filter_lower( val ); MyGreaterEqualFilter filter_greater_equal( val ); ROS_INFO_STREAM_FILTER( &filter_lower, "My filter INFO stream message; val (" = 0" ); 其它类型的消息—单次、定时、组合。 测试控制消息的发出次数。利用ROS_[_STREAM]_ONCE[_NAMED] 函数可以显 示消息的单次输出。无论函数被调用了几次，只有一次输出 for( int i = 0; i < 10; ++i ) { ROS_INFO_STREAM_ONCE( "My once INFO stream message; i = " << i ); } 定时的输出，可以控制该消息在一定时间内可以输出，过了该时间短，即使再次调用该函数 也不会再有输出。第一个参数为时间区域。 for( int i = 0; i < 10; ++i ) {

ROS INFO STREAM THROTTLE 2, "My once INFO stream message;i="<<i )方 ros::Duration(1 ).sleep(); 最后，带名字的，条件的，和一次/定时的消息可以组合使用。 使用rosconsole和rxconsole来在线改变调试Ievel 在ROS的框架中，rosconsole API在前面的实验中己经被用来实现一些功能。更多的功 能可以查看具体API说明。 除了在node中使用API的方式配置日志的记录，ROS还提供了一个图形化界面工具 xconsole,他是rxtools包中的一个部件。在命令行中输入xconsole,在出现的图形化界面中 可以进行查看，检查和配置所有node的日志子系统。 首先，我们运行一个带有日志信息输出的例程 rosrun chapter4 example3 然后在另一个终端中运行rosconsole。将出现以下界面 rxconsole http://ocathost:11311 Message Severity Node Severity Fatal Error warn info Debug se dlear Setup Levels New window. Enebled Include Reges FromMessage Node Location Topics 一旦eample.3例程运行并输出日志信息，可以在rxconsole界面中观察到。 -6-

- 6 - ROS_INFO_STREAM_ THROTTLE ( 2, "My once INFO stream message; i = " << i ); ros::Duration( 1 ).sleep(); } 最后，带名字的，条件的，和一次/定时的消息可以组合使用。 使用rosconsole 和rxconsole 来在线改变调试level 在ROS的框架中，rosconsole API在前面的实验中已经被用来实现一些功能。更多的功 能可以查看具体API说明。 除了在node中使用API的方式配置日志的记录，ROS还提供了一个图形化界面工具 rxconsole，他是rxtools包中的一个部件。在命令行中输入rxconsole，在出现的图形化界面中 可以进行查看，检查和配置所有node的日志子系统。 首先，我们运行一个带有日志信息输出的例程 rosrun chapter4 example3 然后在另一个终端中运行rosconsole。将出现以下界面 一旦eample3例程运行并输出日志信息，可以在rxconsole界面中观察到

ee et。 27里550城发 nAgr3ttet·n3净nFd2g3k25 1两4,5内t5164有4题 oKudld shla 号达小制管起t空绿 e拉体 14252套3Ay5 rt24:t334i计”4ntr1年rvra U制taEk vcinohn ggr风rtt4t"3,pr红+aairgeeirohna字Irna 3ta为3过)：442华3N》 B4 2 nohitipn'tntauw6k1形 rh aa 后4g NIN 。。1有tE+上 4 gm新be《n,1v1# 卷4 urasn4ek rh arolel 12613461VES 435 erapescutn"3W计g'atEtstiurgvsarols年fnru ●C0U3.n235 flz ,7k3222g比C1 a3g350n0为3过h4人，2sw言) awG星签w量的m显rh酸E地？ 园a女Es大aww 里eM tg正wkn8N意wt里00是1e4yn 在以上的表中，有多个列提供信息包括。消息名称，level,产生的node,产生的时间戳， topic名称，和日志打印宏在程序中的位置。 我们可以通过Pause按钮来暂停或者开始接受消息。请暂停一个日志并观察输出界面的 内容。 00 Node:/cxample3 Tmme:135773035B.001145651 Severlty:Info Locatlon:/home/enrique/dev/rosbook/chapter3_tutarlals/src/example3.cpp:main:23 Publlshed Toples:/rosout INFO throttle message. 下面我们测试一下rxconsole的日志筛选功能。最基本的筛选项是levelf值。可以试验如果 只看FATAL和ERROR的日志。会得到如下效果 L-AINUEEEIN181 No HwMYA 5h5 1i4:w2+e室me多：w量EFQ6 另外我吗还可以通过日志消息的内容，node名称，位置和topic名称，来进行滤波，甚至 还可以使用正则表达式，来组合多种筛选项。 测试Clear的消息清除功能。Setup按钮可以用来配置诊断累计的topic名称（默认为 rosout_agg),GUI最多缓存的日志消息条数。其中诊断累计器是一个ROS服务器的一个接 -7

- 7 - 在以上的表中，有多个列提供信息包括。消息名称，level，产生的node，产生的时间戳， topic名称，和日志打印宏在程序中的位置。 我们可以通过Pause按钮来暂停或者开始接受消息。请暂停一个日志并观察输出界面的 内容。 下面我们测试一下rxconsole的日志筛选功能。最基本的筛选项是level值。可以试验如果 只看FATAL和ERROR的日志。会得到如下效果 另外我吗还可以通过日志消息的内容，node名称，位置和topic名称，来进行滤波，甚至 还可以使用正则表达式，来组合多种筛选项。 测试Clear的消息清除功能。Setup按钮可以用来配置诊断累计的topic名称（默认为 rosout_agg），GUI最多缓存的日志消息条数。其中诊断累计器是一个ROS服务器的一个接

收端，可以接受所有的日志消息。这样，我吗可以检查哪个设备发生故障或者他们是怎么运 行的。 Rosout Panel Setup Rosout Topic frosout agg Messages Buffer Size 20000 Cancel ⊙ 最后，我们可以试试为每一个有名称的日志记录器设置level级别。默认情况下，每一个 node都有一个以它的包命名的日志记录器，但是通过前面的NAMED宏我们可以定义一个明 智。在example3的node中我们有ros.chapter4和ros.chapter4.named msg日志记录器。点击 Levels按钮，可以看到以下画面。 这里选择eample.3的node然后选择相应的日志记录器来设置他们的level。注意，还有一 些内容的日志记录器来选择，除了在前面提到的一些日志记录器。 2.检查系统的运行状态 在系统运行时可能有多个node或者多个topic在发布消息或者连接和监听。同 时还有一些node会提供service.。对于一个大的机器人系统，非常需要有一些工具 能查看在一个给定时间点系统的运行情况。OS提供了一些非常基础但是很有用 的工具，能够检测到node图中任何一个错误的发生，也就是能够展现node图的 整体架构。 -8-

- 8 - 收端，可以接受所有的日志消息。这样，我吗可以检查哪个设备发生故障或者他们是怎么运 行的。 最后，我们可以试试为每一个有名称的日志记录器设置level级别。默认情况下，每一个 node都有一个以它的包命名的日志记录器，但是通过前面的NAMED宏我们可以定义一个明 智。在example3的node中我们有ros.chapter4 和ros.chapter4.named_msg 日志记录器。点击 Levels按钮，可以看到以下画面。 这里选择eample3的node然后选择相应的日志记录器来设置他们的level。注意，还有一 些内容的日志记录器来选择，除了在前面提到的一些日志记录器。 2. 检查系统的运行状态 在系统运行时可能有多个node或者多个topic在发布消息或者连接和监听。同 时还有一些node会提供service。对于一个大的机器人系统，非常需要有一些工具 能查看在一个给定时间点系统的运行情况。ROS提供了一些非常基础但是很有用 的工具，能够检测到node图中任何一个错误的发生，也就是能够展现node图的 整体架构

2.1显示nodes,.topic,和service 分别运行以下命令，将看到如何获得正在运行的node列表，以及在给定时间可用的topic 和service。 虽然非常简单，但这是非常有用和鲁棒的。 要获取运行使用的节点列表 rosnode list 所有节点的主题均使用以下内容列出： rostopic list 而且，类似地，所有的服务都显示为： rosservice list 在前面实验中介绍过，利用一些其他的参数，可以通过rosmsg命令查看由特定的节目发 送的消息类型的等。 2.2利用rxgraph在线检查node图 ROS框架提供了一些工具可以生成node图，在该图中可以表示出当前运行的node,以及 node之间的topic的联系。其中主要的一个工具时rxgraph,可以动态的显示系统执行时的node 图，让我们可以看到一个node是怎样加入到系统中来的。 运行以下代码，将例程4和5同时运行。 roslaunch chapter4 example4 5.launch 运行rxgraph可以观察到如下结果。在这张图中剋看到node之间通过topic相互连接，同时 /rosout是ROS的服务node,/rosouttopic是将日志信息发送到服务器中的诊断累计器。在右边 的窗体中可以看到一个节点的详细信息。 O0合xgraph infer Node Lfrovout] /rosoutiget_laggers fntout 、eample4∑le E世文性) 6ct40430 trantport:TCPROS 在工具栏勾选Quieti选项，会出现相对简洁的效果，ROS服务node被滤除了。在观察一 些大系统时这个功能非常的使用。一个node提供或者使用的service可以在右侧的窗体中找到 -9-

- 9 - 2.1 显示nodes，topic，和service 分别运行以下命令，将看到如何获得正在运行的node列表，以及在给定时间可用的topic 和service。 虽然非常简单，但这是非常有用和鲁棒的。 要获取运行使用的节点列表 rosnode list 所有节点的主题均使用以下内容列出： rostopic list 而且，类似地，所有的服务都显示为： rosservice list 在前面实验中介绍过，利用一些其他的参数，可以通过rosmsg命令查看由特定的节目发 送的消息类型的等。 2.2 利用rxgraph在线检查node图 ROS框架提供了一些工具可以生成node图，在该图中可以表示出当前运行的node，以及 node之间的topic的联系。其中主要的一个工具时rxgraph，可以动态的显示系统执行时的node 图，让我们可以看到一个node是怎样加入到系统中来的。 运行以下代码，将例程4和5同时运行。 roslaunch chapter4 example4_5.launch 运行rxgraph可以观察到如下结果。在这张图中剋看到node之间通过topic相互连接，同时 /rosout是ROS的服务node，/rosouttopic是将日志信息发送到服务器中的诊断累计器。在右边 的窗体中可以看到一个节点的详细信息。 在工具栏勾选Quiet选项，会出现相对简洁的效果，ROS服务node被滤除了。在观察一 些大系统时这个功能非常的使用。一个node提供或者使用的service可以在右侧的窗体中找到

ngraph 靠ulel口4 tapics0 g05otrosgnptuns/ ·acclomyn/ /example4 /accel example5 /temp tranip9rtTeiog 一旦系统发生问题，相应的node会变成红色，在这种情况下，选择工具栏中的ALL topics 来显示还没有连接的topics。有些时候产生问题的原因可能非常简单比如topic名字写错了等 等。 当系统运行在分布式环境下时，利用rxgraph可以用来判断远程的node是否被系统识别 到，这是一个很方便的功能。 2.3利用roswtf处理异常问题 ROS还提供了一个能够检查潜在问题的工具。 利用roscdi进入准备要分析的包，然后运行roswtf。在chaptera4中我们会有以下的输出 Packape:chapter3_tutorials Static checks summary: Found 1 warning(s). Harnings are things that nay be just fine,but are sometimes at fault HARNING ROS_IP may be incorrect:ROS_IP [locathost]does not appear to be a local IP address ['127.0.0.1','192.168.1.35']. Found 1 error(s). eRperoluoiaaeehp5roiaeta。wtatgn,pah,/oos/ieeb geonetry_msgs:found flag "-L/opt/ros/fuerte/lib",but no matching "-WL,-rpath,/opt/ros/fuerte/lib" std_nsgs:found flag "-L/opt/ros/fuerte/lib",but no matching "-WL,-rpath,/opt/ros/fuerte/lib" roscpp:found flag"-L/opt/ros/fuerte/lib",but no matching "-WL,-rpath,/opt/ros/fuerte/lib" done analyzing graph running graph rules. .done running graph rules Online checks surnary: ound 3 error(s). ERROR The following nodes should be connected but aren't: /exanple4->/rosout (/rosout) 正常情况下我们希望没有任何错误或者警告，但有时后出现一个警告也是没有太大问题 的。 3.绘制数值量 利用ROS中的通用工具可以非常方便的对一些数值量进行绘制。在绘制时注意，只能对 数值量的数值通道做分别绘制。多于多通道的数值结构体可以使用更好的显示方法来进行显 示，如图像，位姿，方向等。 -10-

- 10 - 一旦系统发生问题，相应的node会变成红色，在这种情况下，选择工具栏中的ALL topics 来显示还没有连接的topics。有些时候产生问题的原因可能非常简单比如topic名字写错了等 等。 当系统运行在分布式环境下时，利用rxgraph可以用来判断远程的node是否被系统识别 到，这是一个很方便的功能。 2.3 利用roswtf处理异常问题 ROS还提供了一个能够检查潜在问题的工具。 利用 roscd进入准备要分析的包，然后运行roswtf。在chapter4 中我们会有以下的输出 正常情况下我们希望没有任何错误或者警告，但有时后出现一个警告也是没有太大问题 的。 3. 绘制数值量 利用ROS中的通用工具可以非常方便的对一些数值量进行绘制。在绘制时注意，只能对 数值量的数值通道做分别绘制。多于多通道的数值结构体可以使用更好的显示方法来进行显 示，如图像，位姿，方向等