rqt_dep用于查看功能包的依赖关系图。

编译ROS时，有时会报错缺某个包，常常是 could not find *Config.cmake 或者 *-config.cmake，我以前一般的解决方法是：sudo apt-get install ros-kinetic-package_name，其实用rosdep更方便。

安装ROS时，会有一个命令 sudo apt-get install python-rosdep，在安装完成后，还会执行

1
2

sudo rosdep init
rosdep update

在src目录下运行rosdep check --from-path package_name，可以查找到所有依赖包。 安装所有依赖包的命令： rosdep install --from-path package_name

rosdep check --from-path src --ignore-src -r -y对整个工作空间检查依赖

最强大的就是rosdep install --from-path src --ignore-src -r -y，在工作空间路径执行，它magically安装了所有 package缺失的依赖项。

pair

map是将key和value放在一起来保存，pair是更一般化地将2个数据组合成一组数据，另外当一个函数需要返回2个数据的时候，可以选择pair。 pair的实现是一个struct，主要的两个成员变量是first 和 second，因为是使用struct不是class(默认访问权限public)，所以可以直接使用这两个成员变量。

头文件 #include <utility>， 类模板：template<class T1,class T2> struct pair。 T1和T2的类型可以不同

1
2
3
4
5
6
7

pair<T1, T2> p1;            //创建一个空的pair对象（使用默认构造），它的两个元素分别是T1和T2类型，采用值初始化。
pair<T1, T2> p1(v1, v2);    //创建一个pair对象，它的两个元素分别是T1和T2类型，其中first成员初始化为v1，second成员初始化为v2。
make_pair(v1, v2);          // 以v1和v2的值创建一个新的pair对象，其元素类型分别是v1和v2的类型。
p1 < p2;                    // 两个pair对象间的小于运算，其定义遵循字典次序：如 p1.first < p2.first 或者 !(p2.first < p1.first) && (p1.second < p2.second) 则返回true。
p1 == p2；                  // 如果两个对象的first和second依次相等，则这两个对象相等；该运算使用元素的==操作符。
p1.first;                   // 返回对象p1中名为first的公有数据成员
p1.second;                 // 返回对象p1中名为second的公有数据成员

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

pair<int ,double> p1;
p1.first = 1;
p1.second = 2.5;

pair<int, double> p2;
p2 = make_pair(1, 1.2);



std::pair<std::string, int> getPreson()
{
    return std::make_pair("Sven", 25);
}
 
std::string name;
int ages;
std::tie(name, ages) = getPreson();
std::cout << "name: " << name << ", ages: " << ages << std::endl;

string的find 函数

find()返回值是元素在string中的位置（下标记录），如果没有找到，那么会返回一个特别的标记npos，是一个很大很大的数。返回值可以看成是一个int型的数

1
2
3
4
5
6
7
8
9
10
11
12
13

string s("12345");
string::size_type  position;
// 返回34 在 s 中的下标位置
position = s.find("34");
//如果没找到，返回一个特别的标志c++中用npos表示，我这里npos取值是4294967295
if (position != s.npos) 
{
    printf("position is : %d\n" ,position);
}
else
{
    printf("Not found\n");
}

在移动机器人的运动过程中，当激光雷达扫描频率较低时，获得的原始扫描数据会发生畸变，并且随着速度的增加变得越来越严重。雷达旋转有顺时针与逆时针2种情况，但导致运动畸变的效果差异并不明显。

1. 激光点数据不是瞬时获得的，雷达扫描时在旋转，有个扫描频率的问题
2. 雷达扫描中，机器人也是运动的。 机器人旋转造成的运动畸变比直线运动大。

比如所用激光雷达的扫描频率为10Hz，当移动机器人以角速度为1 rad/s 进行纯旋转时，在激光雷达扫描一圈得到
一帧数据的时间内（0.1s）移动机器人已经旋转了0.1 rad，约为5.7度，这会影响机器人的定位和路径规划。

由于我以前的机器人是低速机器人，最大线速度才0.4m/s，而雷达的扫描频率至少25Hz，所以雷达不必考虑畸变，实际使用时，从rviz上看，也没发现运动畸变。雷达扫描频率最好达到30Hz以上。但是我现在使用的机器人速度最快1.4m/s，雷达频率才10Hz，这就需要考虑运动畸变了。

使用里程计辅助的方法去除运动畸变。里程计的频率一般比较高，move_base里面默认订阅里程计的频率是20Hz，所以里程计至少也得20Hz，我所用的里程计是50Hz。

平时之所以没有这么严重，是因为有定位程序，比如AMCL或者LOAM

去除激光帧畸变原理：把一帧激光的每个激光点坐标变换到不同时刻的机器人里程计上
里程计辅助方法：imu、轮式里程计
(1) IMU
　　直接测量获得角速度和线速度，但对于机器人位移和角度需要积分
　　更新速度高 1KHz－8KHz
　　具有较高的角速度测量精度，但线加速度精度差
２）轮式里程计
　　更新速度高 100－200Hz
　　（一般机器人轮式里程计200hz，即可认为满足一定的要求，200hz一帧5ms，1.57rad/s下，误差仅为0.45度，可认为机器人没有运动。）

程序流程

1) main-构造函数
订阅scan话题
调用scancallback处理
2) scancallback
scan话题数据提取到vector数组(包括angles,ranges)
vector数组保存到pcl::PointCloud类型的点云中
调用Lidar_Calibration进行畸变矫正
3) Lidar_Calibration
程序对激光雷达数据进行分段线性插值*
调用Lidar_MotionCalibration激光雷达运动畸变去除分段函数*
调用getLaserPose得到激光雷达在odom中的位姿

参考：
基于里程计修复激光雷达数据
激光雷达移动状态下的数据矫正
激光雷达数据效果对比

马氏距离

数据的协方差距离。它是一种有效的计算两个未知样本集的相似度的方法。与欧氏距离不同的是它考虑到各种特性之间的联系（例如：一条关于身高的信息会带来一条关于体重的信息，因为两者是有关联的）并且是尺度无关的（scale-invariant），即独立于测量尺度。

均方根误差

RMSE = Root Mean Square Error=均方根误差。

含义：衡量观测值与真实值之间的偏差。
用途：常用来作为机器学习模型预测结果衡量的标准

CMake加入的一些设置，简化编译结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

set(SIMPLIFY "ON")

if(SIMPLIFY STREQUAL "ON")
    MESSAGE("catkin_make small libfile.so")
    SET(CMAKE_BUILD_TYPE "Release")
    # 删除调试符号
    set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -s")
    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -s")
     
    # 开启空间优化
    if (APPLE)
      set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -Oz")
      set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Oz")
    else ()
      set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -Os")
      set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Os")
    endif ()
     
    # 使用gc-section优化
    set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -ffunction-sections -fdata-sections")
    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -ffunction-sections -fdata-sections")
    set(CMAKE_EXE_LINKER_FLAGS    "${CMAKE_EXE_LINKER_FLAGS}    -Wl,--gc-sections")
    set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -Wl,--gc-sections")
    set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--gc-sections")
endif()

1. 编译选项使用-Os，表示以最小化大小为优化方向，假如链接失败，则修改为-O2

在 CMakeLists.txt 文件中加入以下两行：

1
2

set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -s")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -s -O4 -Wall")

1. 去除-g选项，进而去除调试信息

2. 通过strip裁剪符号及调试信息

3. 只导出必要符号

Linux默认导出所有符号，并不仅仅导出你开放的接口。

定义如下宏：

#define SYMBOL_EXPORT __attribute__ ((visibility("default")))

在你想导出的符号前加上SYMBOL_EXPORT就好。

同时，需要在脚本中增加如下编译选项： -fvisibility = hidden

譬如：导出符号是int add(int a, int b); 那么添加的结果就是SYMBOL_EXPORT int add(int a, int b);

通过这种方式只导出想导出的符号，也可以减小库大小。

default: 用它定义的符号将被导出，动态库中的函数默认是可见的。

hidden: 用它定义的符号将不被导出，并且不能从其它对象进行使用，动态库中的函数是被隐藏的。

default意味着该方法对其它模块是可见的。而hidden表示该方法符号不会被放到动态符号表里，所以其它模块(可执行文件或者动态库)不可以通过符号表访问该方法。这个选项可能导致编译失败，出现undef reference

还有一个选项 -fvisibility = inlines-hidden

• 控制C++特性的使用，如无必要，则不使用C++的高级特性。

• 屏蔽RTTI特性，增加编译选项-fno-rtti。

• 对性能影响不大时，避免使用C++的inline特性，也就是增加编译选项 -fno-inline

• 在不影响使用时，控制对STL的组件的使用。尤其iostream的相关模板类。

示例：

1
2
3
4
5
6
7
8
9
10
11
12

# 关闭C++特性set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-inline")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-exceptions")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-rtti")

set(CMAKE_C_FLAGS_RELEASE     "${CMAKE_C_FLAGS_RELEASE}   -fvisibility = inlines-hidden")
set(CMAKE_CXX_FLAGS_RELEASE   "${CMAKE_CXX_FLAGS_RELEASE} -fvisibility = inlines-hidden")
# 删除调试符号
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -s")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -s")
# 开启空间优化
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -Os")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Os")

使用程序实现父子坐标系的转换

除了直接用static_transform_publisher外，当然可以在程序中实现两个坐标系的转换，以经典的乌龟追乌龟程序为参考，它的tf树是这样的

现在我们写一个这样的程序：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <turtlesim/Pose.h>

void poseCallback(const turtlesim::PosePtr& msg)
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(0,0, -0.24) );  //子坐标系相对父坐标系的位移
  //子坐标系相对父坐标系的旋转，可以用欧拉角和四元数两种形式
  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  //tf::Quaternion q(qx,qy,qz,qw);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "robot_base"));   //父坐标系在前，子坐标系在后
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "camera_base_tf");
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe("/turtle1/pose", 10, &poseCallback);
    ros::spin();
    return 0;
}

使用程序获得坐标系在另一坐标系中的坐标

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "turtle_2_1");
  ros::NodeHandle n;
  tf::TransformListener listener;
  ros::Rate rate(10);
  while(n.ok())
  {
    tf::StampedTransform transform;
    try
    {
      listener.waitForTransform("turtle1","turtle2",ros::Time(0),ros::Duration(3));
      listener.lookupTransform("turtle1","turtle2",ros::Time(0), transform);
    }
    catch(tf::TransformException &ex)
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1).sleep();
      continue;
    }
    ROS_INFO("x: %f", transform.getOrigin().x());
    ROS_INFO("y: %f", transform.getOrigin().y());
    ROS_INFO("z: %f\n", transform.getOrigin().z());
    rate.sleep();
  }
  return 0;
}

这个程序获得的是乌龟2在乌龟1坐标系中的坐标

先运行roslaunch turtle_tf turtle_tf_demo.launch，这是ROS自带的一个tf演示程序，然后马上运行上面的程序。我们会看到乌龟1不动，乌龟2向前者移动，如果运行程序太慢，乌龟2就停止了，二者没有相对运动就捕捉不到tf话题上的消息了

同时我们的程序会输出下面内容

1
2
3
4
5
6
7
8
9
10
11
12

[ INFO] [1551530137.681192434]: x: -1.394780
[ INFO] [1551530137.681317271]: y: -3.510608
[ INFO] [1551530137.681352733]: z: 0.000000

[ INFO] [1551530137.781891361]: x: -1.221200
[ INFO] [1551530137.781970030]: y: -3.391552
[ INFO] [1551530137.782007398]: z: 0.000000

[ INFO] [1551530137.982427762]: x: -1.018356
[ INFO] [1551530137.982512261]: y: -3.122349
[ INFO] [1551530137.982559203]: z: 0.000000
......	//忽略

程序分析

waitForTransform函数的原型有两个，我们用的这个是:

1
2
3
4
5
6
7
8
9

bool Transformer::waitForTransform
(
	const std::string & 	target_frame,
	const std::string & 	source_frame,
	const ros::Time & 		time,	//The time at which to transform
	const ros::Duration & 	timeout,  //How long to block before failing
	const ros::Duration & 	polling_sleep_duration = ros::Duration(0.01),
	std::string * 			error_msg = NULL 
)const

一般用前四个参数就行了，如果开始没有坐标系转换，会报错 “turtle1” passed to lookupTransform argument target_frame does not exist 。如果又有了转换，程序可以正常运行。因为网络问题，如果timeout为0，那会直接报错失败，如果设置太小，也可能捕捉不到转换。使用tf_echo就会发现它要等一点时间才会输出结果，这是一样的道理。

1
2
3
4
5
6
7

void Transformer::lookupTransform
(	
	const std::string & 	target_frame,
	const std::string & 	source_frame,
	const ros::Time & 	time,  //某时间的变换，0表示最近的变换
	StampedTransform & 	transform 
)const

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

std::string tf_error;
// make sure that the transform between base_link and the global frame is available
while (ros::ok()
    && !tf_.waitForTransform(global_frame_, "base_link", ros::Time(), ros::Duration(0.1), ros::Duration(0.01), &tf_error))
{
  ros::spinOnce();
  if (last_error + ros::Duration(5.0) < ros::Time::now())
  {
    ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
             robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
    last_error = ros::Time::now();
  }
  // The error string will accumulate and errors will typically be the same, so the last
  // will do for the warning above. Reset the string here to avoid accumulation.
  tf_error.clear();
}

1
2
3
4

bool Transformer::getParent ( const std::string &   frame_id,
    ros::Time   time,
    std::string &   parent 
)   const