ROS机器人高效编程 | 第3章可视化和调试工具

本文所用代码是在书本源码基础简单修改，本章代码结构：

  RoboTT|
		|-ch2
		|_ch3
			|_bag
		    |   |_2014-07-01-22-54-34.bag
			|
			|_cfg
			|	|_DynamicParam.cfg
			|
			|_config
			|	|_bag_plot.perspective
			|  	|_ch3.config
			|	|_diagnostic_aggregator.yaml
			|	|_example9.rviz
			|	|_example10.rviz
			|	|_example_tf.rviz
			|	
			|_launch
			|	|_ch3_1.launch
			|	|_ch3_6.launch
			|	|_example1_gdb.launch
			|	|_example1_valgrind.launch
			|	|_example2.launch
			|	|_example3.launch
			|	|_example4_5.launch
			|	|_example4_record.launch
			|	|_example7.launch
			|	|_example8.launch
			|	|_example9.launch
			|	|_example10.launch
			|		
			|_output
			|	|_example10.launch
			|		
			|_src
			|	|_ch3_1.cpp
			|	|_ch3_2.cpp
			|	|_ch3_3.cpp
			|	|_ch3_4.cpp
			|	|_ch3_5.cpp
			|	|_ch3_6.cpp
			|	|_ch3_7.cpp
			|	|_ch3_8.cpp
			|	|_ch3_9.cpp
			|	|_ch3_10.cpp
			|		
			|_srv
			|	|_SetSpeed.srv
			|
			|_CMakeLists.txt
			|
			|_package.xml

ROS可视化和调试代码的工具：消息日志系统、诊断消息、可视化和检测工具，展示了正在运行的结点和他们之间的关系，以便检测并解决软硬件问题。

3.2 日志消息

3.2.1 输出日志消息

// 1. 等同c语言的 printf函数
float val = 1.23;
ROS_INFO("My Info message:%f",val);
/
 * 输出内容：[ INFO] [1604578069.045487819]: My Info message:1.230000
 * [ INFO] 消息的级别 
 * [1604577524.746130574] 时间戳
/ 

// 2. 等同c++ cout 
int val =10;
ROS_INFO_STREAM("My INFO message with argument: "<<val);
/
 *输出内容：[ INFO] [1604578506.760442795]: My INFO message with argument:10
/

3.2.2 设置调试消息级别

ROS有5个日志消息标准级别：

DEBUG 		//！ 调试  	green 
INFO		//！ 信息  	white
WARN		//！ 警告 	yellow
ERROR		//！ 错误	red
FATAL		//！ 致命	purple
//-- 语法：
ROS_LEVEL()

3.2.3 为特定节点配置调试消息级别

方式1 | 宏定义

ROSCONSOLE_SEVERITY_DEBUG
ROSCONSOLE_SEVERITY_INFO
ROSCONSOLE_SEVERITY_WARN
ROSCONSOLE_SEVERITY_ERROR
ROSCONSOLE_SEVERITY_FATAL
ROSCONSOLE_SEVERITY_NOTE
//-- 使用方法：
#define ROSCONSOLE_MIN_SEVERITY ROSCONSOLE_SERVERITY_ERROR

方式2 | CMakeList.txt 设置功能包中所有节点的宏

add_definitions(-DROSCONSOLE_MIN_SERVERITY=ROSCONSOLE_SEVERITY_ERROR)

方式3 | 在配置文件中设置

步骤1:新建config/ch3.config，存入以下内容

log4j.logger.ros.ch3=DEBUG

步骤2：新建 launch/ch3_1.launch ，存入以下内容

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <!-- Logger config -->
  <env name="ROSCONSOLE_CONFIG_FILE"
       value="$(find ch3)/config/ch3.config"/>
  <!-- ch3_1 -->
  <node pkg="ch3" type="ch3_1" name="ch3_1" launch-prefix="xterm -e gdb --args" output="screen"/>
</launch>

3.2.4 消息命名

//--格式：
ROS_<LEVEL>_[STREAM]_NAMED
//-- 使用方法1：
double val =3.14;
ROS_INFO_STREAM_NAMED("named_msg","My named INFO stream message;val="val);
/
 *输出：[ INFO] [1604583470.650429289]: My named INFO stream message; val = 3.14
/
//--使用方法2：在config设置级别
log4j.logger.ros.ch3.named_msg=ERROR

3.2.5 按条件显示消息与过滤消息

1.按条件显示消息

//-- 格式
ROS_<LEVEL>[_STREAM]_COND[_NAMED]
//-- 使用方法
double val=3.14 
ROS_INFO_STREAM_COND(val>0.,"My conditional INFO stream message;val ("<<val <<")<0");

2.过滤消息，用户自定义过滤器继承自ros::console::FilterBase 结构体。必须将过滤器作为指针传递给以ROS_[_STREAM]_COND[_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 );
   //-- val小于0时输出
    ROS_INFO_STREAM_FILTER(
    &filter_lower,
    "My filter INFO stream message; val (" << val << ") < 0"
  );

3.2.6 显示消息的方式

显示消息的方式：单次、可调和其他组合
1.单次

// 格式
ROS_<LEVEL>[_STREAM]_ONCE[_NAMED]
// 使用
for(int i=0;i<10;i++)
	{
		ROS_INFO_STREAM_ONCE("My once INFO stream message;i =" <<i);
	}

2.固定频率

// 格式
ROS_<LEVEL>[_STREAM]_THROTTLE[_NAMES]
// 使用
for(int i=0;i<10;i++)
	{
		ROS_INFO_STREAM_THROTTLE(2,"My throttle INFO stream message; i =" <<i); 
	}

2.2.7 运行时修改调试级别

rqt_console对日志的消息进行可视化、过滤和分析
rqt_logger_level 设置节点或者指定日志记录器的日志级别

roscore 
rosrun rqt_console rqt_console  //!! rosconsole GUI
rosrun ch3 ch3_3  //!! 节点  
rosrun rqt_logger_level rqt_logger_level //!! loger level GUI

3.3 检测系统状态

rosnode list		//!! 正在运行的节点
rostopic list 		//!! 正在运行的话题
rosservice list 	//!! 正在运行的服务
rosparam list 		//!! 服务器上的参数 

3.4 设置动态承参数

条件：节点实现了一个动态重配置参数服务
1.cfg/DynamicParam.cfg
2.CMakeList.txt 添加：generate_dynamic_reconfigure_options(cfg/DynamicParam.cfg)
3.cpp 代码实现
具体ch3部分代码
使用：rqt_reconfigure
使用场景，如：Hokuyo激光测距仪的驱动程序、FireWire相机的驱动程序修改帧率、快门速度、亮度等。

roslaunch ch3 ch3_6.launch 
rosrun rqt_reconfigure rqt_reconfigure 

3.5 当出现异常状况时使用roswtf

检测给定功能包中所有元件的潜在问题

//-- 进入ch3功能包
roscd ch3
//-- 运行节点
roslaunch ch3 ch3_6
//--运行roswtf
roswtf
//--安装python-catkin-link
sudo apt-get install python-catkin-link
//--catkin_link 帮助catkin诊断错误（CMakeList.txt package.xml中)
catkin_link -W2 --pkg ch3

3.6 可视化节点诊断

roslaunch ch3 ch3_7.launch 
rosrun rqt_runtime rqt_runtime
rosrun rqt_robot_monitor rqt_robot_monitor

3.7 绘制标量示意图

3.8 图像可视化

3.9 3D可视化

启动：

rosrun rqt_rviz rqt_rviz

参考blog
[1] RVIZ中的fixed frame选项以及“For frame [XX]: Fixed Frame [map] does not exist”