用freopen函数操作输入输出流和读写文件|C++C++ 基础
此方法非常不好，容易造成输入输出流混乱，最好用fopen,fread,fwrite



freopen函数用于重定向输入输出流，freopen(file, "w",stdout)用于将输出流定向到文件file，freopen(file, "r",stdin)是从文件file读文件。

1
2
3
4
5
6
7
8
9
10
11
12
13
FILE* fp = freopen("/home/user/Documents/test.txt","w",stdout);
srand((int)time(NULL));
for(int i=0;i<8;i++)
{
    int num = rand()%100;
    string s = boost::lexical_cast<string>(num);
    cout << s << endl;
}
// fclose(stdout);
freopen("/dev/tty", "w", stdout);

// 在终端运行会输出，在IDE里不会输出
cout<<"**************"<<endl;

代码先将输出流定向到文件test.txt，后面的cout<<s<<endl会将几个随机数写入到文件。
如果之后直接fclose(stdout)，之后的cout输出肯定不管用，此时把输出流关了，对终端和文件都无法输出了。

在使用完freopen之后，如果还需要使用标准输入输出，不能把它们直接fclose，不妨再次重定向，把stdin、stdout重定向到控制台，就能从键盘接受输入、从屏幕输出。在linux中，控制台设备是/dev/tty，所以再次调用freopen。最后的星号只能在终端执行可执行文件时，才会输出。

以上函数对log4cpp的日志和ROS_INFO也有影响，显然log4cpp的本质也是操作输出流，与freopen相同。

读文件

比如文件内容如下：

1
2
3
4
5
6
31
96
5
86
98
20

读其中数字的代码：

1
2
3
4
5
6
7
8
freopen(file, "r", stdin);
vector<int> data(6,0);

for(int i = 0; i < 6; ++i)
{
    cin >> data[i];
    cout << "data: " << data[i] ;
}

但是文件中有其他内容时，会读取失败，例如：

第4个元素赋值会成为0，值之后的数据都不正常

传递数组给函数|C++C++ 基础

数组不能赋值给数组

1
2
3
int a[3] = {1,2,3};
int b[3];
b=a;

编译器报错, 数组不能直接赋值, 可以使用std::copy()或手工循环赋值, 但是不可以直接把一个数组赋值给另外一个数组.

但是std::array和std::vector是可以的 
1
2
std::array<int, 5> a = {1,2,3,4,5};
std::array<int, 5> b = a;


形式参数是一个已定义大小的数组：

1
2
void myFunction(int param[10])
{}

形式参数是一个未定义大小的数组：

1
2
void myFunction(int param[])
{}

函数而言，数组的长度是无关紧要的，因为 C++ 不会对形式参数执行边界检查。

二维数组做形参

1
2
3
4
5
6
7
8
9
10
void func1(int  iArray[][10])
{

}

int main()
{
    int array[10][10];
    func1(array);
}

形参声明一定要给出第二个维度的大小，否则编译不过。

轮速里程计|路径规划move_base分析

里程计是衡量我们从初始位姿到终点位姿的一个标准，通俗的说，要实现机器人的定位与导航，就需要知道机器人行进了多少距离，是往哪个方向行进的， odom的原点是机器人启动的位置。

里程计一个很重要的特性，是它只关心局部时间上的运动，多数时候是指两个时刻间的运动。当我们以某种间隔对时间进行采样时，就可估计运动物体在各时间间隔之内的运动。由于这个估计受噪声影响，先前时刻的估计误差，会累加到后面时间的运动之上

odom坐标系是在机器人运动前以当前观测到的数据建立的临时的坐标系，避免远离map原点而造成误差。比如轮子的编码器或者视觉里程计算法或者陀螺仪和加速度计。odom坐标系原则上是固定的，但可能会随着机器人的运动漂移，这就是累计误差，漂移导致odom不是一个很有用的长期的全局坐标，但是一个有用的短期坐标。在odom坐标系下机器人的位置必须是连续变化的，不能有突变和跳跃，比如人不能把机器人搬到其他地方。

里程计中也存在线速度角速度，但是这个线速度角速度是通过距离除以时间得到的，没有IMU的精确．

里程计的发布频率不能低，move_base里面默认订阅里程计的频率是20Hz，所以里程计至少也得20Hz，目前机器人的为50Hz

运动模型

机器人在世界坐标系中的位置.png

机器人在世界坐标系中的位置.png

机器人的位置X,Y可以看做车在极小时间内的位置增量在X,Y方向的分量的积分。我们采用速度推算方式确定机器人的轨迹，在极小时间Δt内，以速度V移动的距离是Δd，分解到两坐标轴可以得到

这样不断累积，就可以得到任意时间的位置：
2020-03-21_094244.png

2020-03-21_094244.png

  • Δt是我们自己取的，比如10ms，可以在ROS中使用ros::Rate实现

  • V和ω并不是我们指定的速度，而是由指定速度和角速度根据航迹推演之后，分配到两个驱动轮上的速度Vl和Vr，再从硬件层根据Kinematics返回的速度Vx和ω。由于驱动轮不是全向轮，所以Vy是0

  • 至于偏航角，有两种方法获取：编码器和IMU。编码器很依赖精度，效果不如IMU。使用IMU的话，θ就是其返回的偏航角Yaw，IMU上电时的Yaw为0，与小车航向一致，只是还要根据公式转换到(-π，π]

tf关系

用到的tf父子关系为： map–>odometry–>base_link ，随着机器人的移动，我们需要生成机器人的里程计信息，如此可以提供机器在map中大致的位置，一般在一个节点里发出odometry->base_link的tf广播，它们的转换信息是每时每刻都在发布的。

odom的原点是机器人启动的位置，因为每次机器人启动的位置不同，所以一开始odom在map上的位置或转换矩阵是未知的，但是AMCL可以根据最佳粒子的位置推算出map->odom（就是说通过最佳粒子推算出机器人在地图的中的位置）的tf转换信息并发布到tf主题上，这样三者的tf父子关系就建立起来了，我们也就知道了base_link在map中的坐标。

不管机器人在哪启动，odom和map坐标系之间的tf理论上是不变的，发生的改变就是里程计的累计误差

一般来说map坐标系的坐标是通过传感器的信息不断的计算更新而来。比如激光雷达，视觉定位等等。因此能够有效的减少累积误差，但是也导致每次坐标更新可能会产生跳跃。map坐标系是一个很有用的长期全局坐标系。但是由于坐标会跳跃改变，这是一个比较差的局部坐标系（不适合用于避障和局部操作）。

map->odom变换代表了对里程计误差的纠正

odom话题

odom消息成员：

1
2
3
4
std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

里程计向局部路径规划器传递当前机器人的速度，消息中的速度也在TrajectoryPlannerROS对象中的代价地图包含的robot_base_frame参数表示的坐标系中，一般就是base_link，这个参数是在全局代价地图的yaml文件中。



在rviz中添加Odometry，最好去掉它的Covariance项后面的勾，也就是在Odometry显示中不启用Covariance信息，否则将导致显示效果很难看

代码如下，对不同的机器人，获取线速度和角速度的方法不同，这里没有用到IMU

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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <boost/shared_ptr.hpp>
#include <geometry_msgs/Twist.h>

static ros::Publisher *odomPubPtr;
double x = 0;
double y = 0;
double theta = 0;
ros::Time current_time;
ros::Time last_time;

geometry_msgs::TransformStamped odom_trans;
nav_msgs::Odometry odom;

void callback(const geometry_msgs::TwistConstPtr& velocity)
{
    static tf::TransformBroadcaster odom_broadcaster;
    double vx, vy, omega;

    current_time = ros::Time::now();
    
    vx = velocity->linear.x;
    omega = velocity->angular.z;
    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
    double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
    double delta_th = omega * dt;

    x += delta_x;
    y += delta_y;
    theta += delta_th;

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);
    
    odom_trans.header.stamp = current_time;
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    odom_broadcaster.sendTransform(odom_trans);
    odom.header.stamp = current_time;

    //set the position
    if(fabs(x) < 0.0001 )  x=0;
    if(fabs(y) < 0.0001 )  y=0;
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = omega;

    odomPubPtr->publish(odom);
    ROS_INFO_ONCE(" 开始发布里程计信息 ");
    last_time = current_time;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "odometry_publisher");

    ros::NodeHandle n;
    setlocale(LC_ALL,"");

    last_time = ros::Time::now();
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";

    ros::Subscriber sub = n.subscribe<geometry_msgs::Twist>("velocity",20,callback);
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    odomPubPtr = &odom_pub;
    ros::spin();
}

这里的里程计频率就由话题velocity的频率决定了，因为是订阅建立后阻塞了，回调函数就一直执行

如果像这样写，每次都要通过网络获取话题的消息，会明显降低里程计的频率

1
2
3
4
5
6
7
8
9
10
boost::shared_ptr<geometry_msgs::Twist const> velEdge;    // 必须有const
velEdge = ros::topic::waitForMessage<geometry_msgs::Twist>("velocity",ros::Duration(1));
if(velEdge != NULL)
{
    velocity = *velEdge;
    vx = velocity.linear.x;
    omega = velocity.angular.z;
}
else
    ROS_WARN("linear and angular velocity not received !");

参考：里程计的信息及对其标定
tf between /map and /odom is not static?

容器中的iterator|C++C++ 模板与STL

先看vector的部分源码，只看变量声明：

1
2
3
4
5
6
7
8
9
10
11
12
13
template <class T, class Alloc = alloc>  // 预设使用 alloc 为配置器
class vector
{
public:
  typedef T value_type;
  typedef value_type* pointer;
  typedef value_type* iterator;
  typedef const value_type* const_pointer;
  typedef const value_type* const_iterator;
  typedef value_type& reference;
  typedef const value_type& const_reference;
  ...  
}

容器的迭代器iterator其实是类模板，不是一个指针，但实现了指针的功能，不能赋值为NULL，也不能与NULL比较。迭代器采用了iterator模式，类似于数据库中的游标,它屏蔽了底层存储空间的不连续性，在上层使容器元素维持一种连续的假象，迭代器代表的是元素在容器中的相对位置。

如果迭代器iterator要初始化或者做空值比较，需要使用end()，它默认指向容器最后一个元素的下一个位置，可以理解为空．如果非要对iterator进行初始化的话，可以将iterator指向end()函数即可。 判断是否为空即与end()函数做比较即可

看下面代码：

1
2
3
4
5
6
7
8
9
10
std::vector<int> vec;
vec.push_back(1);
vec.push_back(3);
vec.push_back(5);
vec.push_back(7);
std::vector<int>::iterator it= vec.end();
qDebug()<<*(--it);		// 像游标
it++;
if(it == vec.end())
    qDebug()<<"vec end";

运行结果为:

1
2
7
vec end

迭代器失效

迭代器指向容器中的某个或某些元素的存储发生了变化，使之成为无效迭代器，使用无效迭代器和无效指针一样危险。

解决迭代器失效问题有两种方式：

  1. 使用迭代器时重新获取迭代器

  2. 在修改容器前为其预留足够的空闲空间，以避免存储空间重分配。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
vector<int> l;
l.push_back(1);
l.push_back(2);
l.push_back(3);
l.push_back(4);
l.push_back(5);
l.push_back(6);

vector<int>::iterator it = l.begin();
vector<int>::iterator end = l.end();
Print(l);
cout<<"size: "<<l.size()<<"  capacity: "<<l.capacity()<<endl;   　// 6   8
cout<< "begin: "<<*it <<endl;       // 1
cout<< "end: "<< *(--end) <<endl;    // 6
// 再插入元素
l.push_back(7);
l.push_back(8);
//    l.push_back(9);

Print(l);
cout<<"size: "<<l.size()<<"  capacity: "<<l.capacity()<<endl;  // 8   8
cout<<"after push_buck begin: "<< *it <<endl;
cout<<"after push_buck end: "<< *(end) <<endl;  // 6

运行结果：

1
2
3
4
5
6
7
8
9
1 2 3 4 5 6 
size: 6  capacity: 8
begin: 1
end: 6

1 2 3 4 5 6 7 8 
size: 8  capacity: 8
after push_buck begin: 1
after push_buck end: 6

可以看出进行push_back后，end返回的迭代器肯定是失效的，容器内部没有对其进行更新，erase也是同样结果．

上面代码加上l.push_back(9);后，运行结果成了这样：

1
2
3
4
5
6
7
8
1 2 3 4 5 6 
size: 6  capacity: 8
begin: 1
end: 6
1 2 3 4 5 6 7 8 9 
size: 9  capacity: 16
after push_buck begin: 0
after push_buck end: 6

我们往vector插入6个元素，size就是6，但vector分配的容量要稍大一些．如果再插入3个元素，就超出了容量，vector会先分配一个2倍大的内存，然后把原来的元素都拷贝过来，释放原内存，这样迭代器就像悬垂指针一样失效了．当删除容器内的数据时，其存储空间不释放

有些资料说重新分配内存是增加原内存的一半，估计是STL版本和编译器版本不同的缘故，我测试的vector的容量都是2的次方

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

vector<int> l;
l.push_back(1);
l.push_back(2);
l.push_back(3);
l.push_back(4);
l.push_back(5);
l.push_back(6);

vector<int>::iterator it = l.begin();
vector<int>::iterator end = l.end();
Print(l);
cout<<"size: "<<l.size()<<"  capacity: "<<l.capacity()<<endl;
cout<< "begin: "<<*it <<endl;
cout<< "end: "<< *(--end) <<endl<<endl;

l.erase(++it);
l.erase(++it);

Print(l);
cout<<"size: "<<l.size()<<"  capacity: "<<l.capacity()<<endl;
cout<<"after erase begin: "<< *it <<endl;
cout<<"after erase end: "<< *(end) <<endl;

运行结果：

1
2
3
4
5
6
7
8
9
1 2 3 4 5 6 
size: 6  capacity: 8
begin: 1
end: 6

1 3 5 6 
size: 4  capacity: 8
after erase begin: 5
after erase end: 6

显然begin是失效了

  • vector的push_back操作可能没事，但是一旦引发内存重分配，所有迭代器都会失效
  • vector的insert操作插入点之后的所有迭代器失效，但一旦发生内存重分配，所有迭代器都会失效
  • vector的erase操作插入点之后的所有迭代器失效
  • vector的reserve操作所有迭代器失效（导致内存重分配）

对于序列式容器(如vector,deque)，删除当前的iterator会使后面所有元素的iterator都失效。这是因为vetor,deque使用了连续分配的内存，删除一个元素导致后面所有的元素会向前移动一个位置。但是erase方法可以返回下一个有效的iterator

  1. 对于节点式容器(map, list, set)元素的删除，插入操作会导致指向该元素的迭代器失效，其他元素迭代器不受影响；
  2. 对于顺序式容器(vector，string，deque)元素的删除、插入操作会导致指向该元素以及后面的元素的迭代器失效

迭代器的自增减

迭代器可以一直自增或自减，可以超出begin()和end()

1
2
3
4
5
6
7
8
9
10
11
vector<int> poses;
poses.push_back(1);

auto iter = poses.end();
iter++;
iter++;
iter++;
cout << " iter - begin: " << iter - poses.begin() << endl <<
        " iter - end: " << iter - poses.end() << endl;
cout << "poses size: " << poses.size() << endl;
cout << "iter pose: " << *iter << endl;   // 不确定的结果

运行结果:
1
2
3
4
iter - begin: 4
iter - end: 3
poses size: 1
iter pose: 0

超出范围的迭代器，不要取对应的元素值，否则就是bug，最好加判断 if(iter - poses.end() >0)

Boost教程（五）bind，function和signal|C++Boost

bind

使用boost实现回调函数常常用的是bind，bind第一个参数可以接受函数对象、函数指针（包括成员函数），bind最多可以接受9个参数，返回一个函数对象，具有operator()，而且类型可以自动推导。对于类的成员函数，需要传入一个类的对象（或引用、指针），这样就剩8个参数可用了。

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
class Demo
{
public:
    int add(int a, int b) {return a+b; }
    static int add_2(int a, int b) {return a+b;}
};

int add_3(int a, int b) {return a+b;}

int main()
{
    Demo d;
    int a = 1, b=4;
    // 类成员函数，d做对象，加&是函数指针
    boost::function<int(int,int)> func = boost::bind(&Demo::add, d, a, b);
    // 静态成员函数，不用&
    boost::function<int(int,int)> func_2 = boost::bind(Demo::add_2, _1, _2);
    // 普通函数
    boost::function<int(int,int)> func_3 = boost::bind(&add_3, _1, _2);
    // 返回函数对象，有operator ()
    cout<<"add: "<<func(a,b) <<endl;
    cout<<"static add: "<<func_2(a,b) <<endl;
    cout<<"function add: "<<func_3(a,b) <<endl;
    return 0;
}

这里的func_2,func_3中的bind用了占位符的形式，在ROS中有一个很重要的应用就是在main函数中给变量赋值，在话题订阅的回调函数中，可以获得此变量的值，用的就是bind，而且用到占位符，在回调执行时才传入值。参考：如何让回调函数与main函数互相传值

function

function可以理解为函数对象的容器，也像一种智能的函数指针，可以配合bind使用，作为bind表达式的结果，可以用于回调函数机制，而且具有很大的弹性。

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
32
int add(int a, int b)
{
    return a+b;
}

class Demo
{
public:
    int add_2(int a, int b)
    {
        return a+b;
    }
};

int main()
{
    boost::function<int(int, int)> func, func_2;
    func = add;	 // 函数对象
    cout << sizeof(func) <<endl;	// 32
    if(func)	// 像智能指针
        cout<< func(4, 2) <<endl;
    func.clear();	// 清空函数对象，这次像容器，也可以用func=0，这像指针
    assert(func.empty());	// 是否为空，也是像容器的特性

    // 对于类成员函数，只能经过bind
    int a=3, b=1;
    Demo d;
    func_2 = boost::bind(&Demo::add_2, d, a, b);
    cout<<  func_2( a, b)<<endl;

    return 0;
}

调用空的function对象会抛出bad_function_call异常，所以在使用function前最好检测有效性，就像指针一样。从function的一些成员函数来看，它确实既像容器又像指针。

两个function对象不能使用==和!=比较，这是特意的。function对象体积比一般函数指针大，速度慢一点。

signal

boost的signal和Qt的signal道理是一样的，都是观察者模式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
#include <boost/signals2.hpp>
void slot1(int n)
{
    cout<<"slot1: "<<n<<endl;
}

void slot2(int n)
{
    cout<<"slot2: "<<n*2<<endl;
}

int main()
{
    signals2::signal<void(int)> sig;    // 模板与槽函数的声明对应
    sig.connect(&slot1);
    sig.connect(&slot2,  signals2::at_front);   // slot2函数放到前面，默认是插到
    // sig.disconnect_all_slots();      // 解绑所有槽函数
    sig(7);                 // 触发信号，调用槽函数
    return 0;
}

运行结果：

1
2
slot2: 14
slot1: 7

move_base分析（一）使用GetPlan获得路径规划|路径规划move_base分析

ROS导航模块的框架如下：

要获得路径规划需要用到move_base中的一个服务make_plan，它就是用于获得从当前位姿到目标位姿的路径，其服务类型为nav_msgs/GetPlan。

nav_msgs/GetPlan.srv成员如下：

1
2
3
4
5
6
7
geometry_msgs/PoseStamped start	  // 起始位姿
geometry_msgs/PoseStamped goal		// 终点位姿

# 如果目标被阻挡了，规划失败时，规划器能到达的位置在x和y方向约束多少米
float32 tolerance
---
nav_msgs/Path plan  		// 服务端返回的路径

其中nav_msgs/Path是一个位姿数组，代表了机器人跟随的路径，定义：

1
2
Header header
geometry_msgs/PoseStamped[] poses

其实思路很简单，写一个service的客户端程序，通信move_base的get_plan，service的请求部分就是起始点的位姿， header.frame_id永远是map，返回值是路径，也是位姿数组。

代码如下：

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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
//路线规划代码
#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH	// c++没有foreach

void fillPathRequest(nav_msgs::GetPlan::Request &request)
{
    request.start.header.frame_id ="map";
    request.start.pose.position.x = 12.378;//初始位置x坐标
    request.start.pose.position.y = 28.638;//初始位置y坐标
    request.start.pose.orientation.w = 1.0;//方向
    request.goal.header.frame_id = "map";
    request.goal.pose.position.x = 18.792;//终点坐标
    request.goal.pose.position.y = 29.544;
    request.goal.pose.orientation.w = 1.0;
    request.tolerance = 0.5;	//如果不能到达目标，最近可到的约束
}
//路线规划结果回调
void callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)
{
    //执行实际路径规划器
    if (serviceClient.call(srv))
    {
        //srv.response.plan.poses 为保存结果的容器，遍历取出
        if (!srv.response.plan.poses.empty())	// empty()判断
        {
            forEach(const geometry_msgs::PoseStamped &p, srv.response.plan.poses)
            {
            	// 一系列点的位姿
                ROS_INFO("x = %f, y = %f", p.pose.position.x, p.pose.position.y);
            }
        }
        else
        {
            ROS_WARN("Got empty plan");
        }
    }
    else {
        ROS_ERROR("Failed to call service %s - is the robot moving?",
                  serviceClient.getService().c_str());
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "make_plan_node");
    ros::NodeHandle nh;
    // Init service query for make plan
    //初始化路径规划服务，服务名称为"move_base_node/make_plan"
    std::string service_name = "move_base_node/make_plan";
    //等待服务空闲，如果已经在运行这个服务，会等到运行结束。
    while (!ros::service::waitForService(service_name, ros::Duration(3.0))) {
        ROS_INFO("Waiting for service move_base/make_plan to become available");
    }
    // 服务的客户端，类型就是 nav_msgs::GetPlan
    ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true);
    if (!serviceClient) {
        ROS_FATAL("Could not initialize get plan service from %s",
                  serviceClient.getService().c_str());
        return -1;
    }
    nav_msgs::GetPlan srv;
    //请求服务：规划路线
    fillPathRequest(srv.request);
    if (!serviceClient) {
        ROS_FATAL("Persistent service connection to %s failed",
                  serviceClient.getService().c_str());
        return -1;
    }
    ROS_INFO("conntect to %s",serviceClient.getService().c_str());
    callPlanningService(serviceClient, srv);
}

有时路径规划失败会报错：

ROS话题通信原理|ROSROS程序和源码分析

发布者的代码一般是这样的：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
ros::init(argc,argv, "Pub");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::Int8>("topic", 50, false);
ros::Rate rate(1);

std_msgs::Int8 msg;

srand(time(NULL));

while(ros::ok())
{
	msg.data = rand()%100;
	pub.publish(msg);
	ROS_INFO("msg:%d",msg.data);

	ros::spinOnce();
	rate.sleep();
}

注册话题

advertise函数的源码：

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
Publisher NodeHandle::advertise(AdvertiseOptions& ops)
{
   ops.topic = resolveName(ops.topic);
   if (ops.callback_queue == 0)
   {
     if (callback_queue_)
     {
       ops.callback_queue = callback_queue_;
     }
     else   // 实际执行这里
     {
        // spinOnce() 就是 g_global_queue->callAvailable(ros::WallDuration());
       ops.callback_queue = getGlobalCallbackQueue();
     }
   }
   SubscriberCallbacksPtr callbacks(boost::make_shared<SubscriberCallbacks>(ops.connect_cb, ops.disconnect_cb,  ops.tracked_object, ops.callback_queue));
 
   if (TopicManager::instance()->advertise(ops, callbacks))
   {
     Publisher pub(ops.topic, ops.md5sum, ops.datatype, *this, callbacks);
     {
       boost::mutex::scoped_lock lock(collection_->mutex_);
       // `collection_`在NodeHandle构造函数中初始化为0，也就是空指针，在NodeHandle::construct中分配内存
       // impl_是boost::shared_ptr< Impl >， pubs_ 是std::vector< Publisher::ImplWPtr >
       collection_->pubs_.push_back(pub.impl_);
     }
     return pub;    // 执行这里
   }
   return Publisher();
}

NodeHandle::advertise的流程.png
NodeHandle::advertise的流程.png

NodeHandle部分成员：

1
2
3
4
5
6
7
class NodeHandle
{

private:
    CallbackQueueInterface *  callback_queue_;
    NodeHandleBackingCollection *   collection_;
};

collection_在NodeHandle构造函数中初始化为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
class NodeHandleBackingCollection
{
public:
    typedef std::vector< Publisher::ImplWPtr >  V_PubImpl;
    typedef std::vector< ServiceClient::ImplWPtr >  V_SrvCImpl;
    typedef std::vector< ServiceServer::ImplWPtr >  V_SrvImpl;
    typedef std::vector< Subscriber::ImplWPtr >   V_SubImpl;
 
public:
    ServiceManagerPtr   keep_alive_service_manager = ServiceManager::instance()
    TopicManagerPtr   keep_alive_topic_manager = TopicManager::instance()
 
    boost::mutex  mutex_;
    V_PubImpl   pubs_;
    V_SrvCImpl  srv_cs_;
    V_SrvImpl   srvs_;
    V_SubImpl   subs_;
};


//一个简单的广播话题的函数， 至少传入两个参数，topic:话题名字，必须独一无二，不能重复， queue_size:发送到subscriber的消息缓存数量
template <class M>
Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false){
    //话题广播时的配置选项
    AdvertiseOptions ops;
    //n.advertise<std_msgs::String>("topic_m",1000)语句运行时<M>为: <std_msgs::String>
    //看不懂， 貌似在调用对象ops的成员函数init()，估计类似于opt.init<M>(topic, queue_size);
    ops.template init<M>(topic, queue_size);
    ops.latch = latch;
    return advertise(ops);//又调用了下面的advertise()函数
}

AdvertiseOptions定义如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/*  AdvertiseOptions是一个类，在/opt/ros/kinetic/include/ros/advertise_options.h
 * \brief Constructor
 * \param _topic Topic to publish on
 * \param _queue_size Maximum number of outgoing messages to be queued for delivery to subscribers
 * \param _md5sum The md5sum of the message datatype published on this topic
 * \param _datatype Datatype of the message published on this topic (eg. "std_msgs/String")
 * \param _connect_cb Function to call when a subscriber connects to this topic
 * \param _disconnect_cb Function to call when a subscriber disconnects from this topic
 */
AdvertiseOptions(const std::string& _topic, uint32_t _queue_size, const std::string& _md5sum,
                 const std::string& _datatype, const std::string& _message_definition,
                 const SubscriberStatusCallback& _connect_cb = SubscriberStatusCallback(),
                 const SubscriberStatusCallback& _disconnect_cb = SubscriberStatusCallback())
: topic(_topic)
, queue_size(_queue_size)
, md5sum(_md5sum)
, datatype(_datatype)
, message_definition(_message_definition)
, connect_cb(_connect_cb)
, disconnect_cb(_disconnect_cb)
, callback_queue(0)
, latch(false)
, has_header(false)
{}

发布话题：

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
template <typename M>
  void publish(const M& message) const
{
  using namespace serialization;
  namespace mt = ros::message_traits;

  if (!impl_)
    {
      ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher");
      return;
    }

  if (!impl_->isValid())
    {
      ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
      return;
    }
    // 验证消息和发布者的类型是否对应
  ROS_ASSERT_MSG(impl_->md5sum_ == "*" || std::string(mt::md5sum<M>(message)) == "*" || impl_->md5sum_ == mt::md5sum<M>(message),
                 "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]",
                 mt::datatype<M>(message), mt::md5sum<M>(message),
                 impl_->datatype_.c_str(), impl_->md5sum_.c_str());

  SerializedMessage m;
  publish(boost::bind(serializeMessage<M>, boost::ref(message)), m);
}

接着再调用

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
void Publisher::publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const
{
  // 验证publisher合法性
   if (!impl_)
   {
     ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
     return;
   }
   if (!impl_->isValid())
   {
      ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
     return;
   }
  TopicManager::instance()->publish(impl_->topic_, serfunc, m);
}

然后是

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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
void TopicManager::publish(const std::string& topic, const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m)
{
  boost::recursive_mutex::scoped_lock lock(advertised_topics_mutex_);
  if (isShuttingDown())
  {
    return;
  }

  PublicationPtr p = lookupPublicationWithoutLock(topic);
  if (p->hasSubscribers() || p->isLatching())
  {
    ROS_DEBUG_NAMED("superdebug", "Publishing message on topic [%s] with sequence number [%d]", p->getName().c_str(), p->getSequence());

    // Determine what kinds of subscribers we're publishing to.  If they're intraprocess with the same C++ type we can  do a no-copy publish.
    bool nocopy = false;
    bool serialize = false;
    // We can only do a no-copy publish if a shared_ptr to the message is provided, and we have type information for it
    if (m.type_info && m.message)
      p->getPublishTypes(serialize, nocopy, *m.type_info);
    else
      serialize = true;
    
    if (!nocopy)
    {
      m.message.reset();
      m.type_info = 0;
    }
    if (serialize || p->isLatching())
    {
      SerializedMessage m2 = serfunc();
      m.buf = m2.buf;
      m.num_bytes = m2.num_bytes;
      m.message_start = m2.message_start;
    }
    p->publish(m);

    // If we're not doing a serialized publish we don't need to signal the pollset.  The write()
    // call inside signal() is actually relatively expensive when doing a nocopy publish.
    if (serialize)
    {
      poll_manager_->getPollSet().signal();
    }
  }
  else
  {
    p->incrementSequence();
  }
}

探索ROS中的XML-RPC机制（二）客户端|ROSROS程序和源码分析

HelloClient

客户端程序如下：

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
32
#include "xmlrpcpp/XmlRpc.h"
#include <iostream>
using namespace XmlRpc;

int main(int argc, char* argv[])
{
  if (argc != 3) {
    std::cerr << "Usage: HelloClient serverHost serverPort\n";
    return -1;
  }
  int port = atoi(argv[2]);
  XmlRpc::setVerbosity(5);

  // Use introspection API to look up the supported methods
  XmlRpcClient c(argv[1], port);
  XmlRpcValue noArg, result;
  for (int i = 0; i < 2000; i++)
  {
    if (c.execute("system.listMethods", noArg, result))
      std::cout << "\nMethods:\n " << result << "\n\n";
    else
      std::cout << "Error calling 'listMethods'\n\n";
  }
  // Call the HelloName method
  oneArg[0] = "Chris";
  if (c.execute("HelloName", oneArg, result))
    std::cout << result << "\n\n";
  else
    std::cout << "Error calling 'HelloName'\n\n";

  return 0;
}

看完服务端，客户端程序就更简单了，XmlRpcClient的构造函数是XmlRpcClient(const char* host, int port, const char* uri=0);，第一个参数是运行服务端程序的远方主机的IP（Server在本机时可用localhost），第二个是端口。

看类XmlRpcServer会发现自带了两个系统methods：system.listMethods和system.methodHelp，各对应一个类，我们可以用客户端直接调用这两个method。

在执行HelloName时需要使用另一个重要的类XmlRpcValue，对若干基本的C++的数据类型进行了封装，涉及的主要数据类型包括如下几个：

1
2
3
4
5
6
7
8
bool          asBool;
int           asInt;
double        asDouble;
struct tm*    asTime;
std::string*  asString;
BinaryData*   asBinary; //typedef std::vector<char> BinaryData;
ValueArray*   asArray;  //typedef std::vector<XmlRpcValue> ValueArray;
ValueStruct*  asStruct; //typedef std::map<std::string, XmlRpcValue> ValueStruct;

几个重要函数：

1
2
3
4
5
6
7
8
9
bool fromXml(std::string const& valueXml, int* offset);
std::string toXml() const;

bool valid() const
Type const &getType() const
// XML解码等一系列函数
bool boolFromXml(std::string const& valueXml, int* offset);
// XML编码等一系列函数
std::string boolToXml() const;


execute函数

执行远程服务端的方法，参数含义一目了然。如果请求发送成功且受到结果，函数返回true。
这是个同步(阻塞)的实现，直到客户端收到回复或错误才返回，否则一直阻塞。使用isFault()函数判断结果是否错误。

大致说一下函数的流程，先是执行setupConnection，与服务端建立连接．然后是生成要发送的XML文本在函数XmlRpcClient::generateRequest，处理了method和参数值．

客户端执行execute后，事件分发器执行XmlRpcClient::handleEvent，处理服务端的回复，主要调用下面三个函数：

  • 首先是XmlRpcClient::writeRequest()，日志从attempt 1 到 XmlRpcClient::writeRequest: wrote 258 of 258 bytes
  • 然后是XmlRpcClient::readHeader()，日志一直到 client read conten length: 122
  • 最后是XmlRpcClient::readResponse()，日志从XmlRpcClient::readResponse (read 122 bytes) 到 response的XML文本

现在回到execute函数，打出日志XmlRpcClient::execute: method HelloName completed

服务端回复的结果赋给result是在XmlRpcClient::parseResponse，这个函数里解析XML文本取出结果．

最后调用XmlRpcClient的析构函数，实际就是close()




整个通信过程中，客户端发的请求是：

1
2
3
4
5
6
7
8
9
POST /RPC2 HTTP/1.1                 # HTTP 1.1默认进行持久连接 Keep-Alive
User-Agent: XMLRPC++ 0.7        # 客户端浏览器类别
Host: 192.168.10.14:8888
Content-Type: text/xml               # 这次类型是text/xml
Content-length: 138                     # 发送请求的长度

<?xml version="1.0"?>
<methodCall><methodName>HelloName</methodName>
<params><param><value>Chris</value></param></params></methodCall>

服务端回复的是：

1
2
3
4
5
6
7
8
9
HTTP/1.1 200 OK
Server: XMLRPC++ 0.7            # web服务器软件名称
Content-Type: text/xml
Content-length: 122             # 响应体的长度

<?xml version="1.0"?>
<methodResponse><params><param>
    <value>Hello, Chris</value>
</param></params></methodResponse>

探索ROS中的XML-RPC机制（一）概述和服务端|ROSROS程序和源码分析

ROS节点的通信机制是基于XML-RPC协议，这是一个远程调用的协议，说白了也是一种进程间通信的方式，让一个进程A调用进程B上的函数，不可能直接调用，只能是将进程A的参数按一定协议封装传到进程B，进程B调用函数处理后，将返回值再封装成XML返回至进程A.

Xml-RPC是一个远程过程调用的分布式计算协议，通过XML将调用函数封装，并 使用HTTP协议作为传送机制。 RPC采用C/S模式。请求程序就是一个客户机，而服务提供程序就是一个服务器。首先，客户机调用进程发送一个有进程参数的调用信息到服务进程，然后等待应答信息。在服务器端，进程保持睡眠状态直到调用信息到达为止。当一个调用信息到达，服务器获得进程参数，计算结果，发送答复，然后等待下一个调用信息，最后，客户端调用进程接收答复信息，获得进程结果，然后调用执行继续进行。

XML-RPC不支持在socket里实现用户轮询代码，用户要轮转一个额外的线程去做这部分工作。使用XML-RPC时，所有使用都通过XMLRPSManager组件，所有与master的通信都通过master::execute函数。



XML-RPC的优点：简单、轻量；XML编码，可读性强；支持多语言多平台；

缺点：

  1. 对字符的编码较弱，中文编码可能得需要通过base64，所以ROS话题不能是中文
  2. 浪费带宽，比如就传递两个参数，需要发送一大堆无用的xml节点
  3. 对复杂数据结构支持不够好
  4. 实时性不够好，不如基于protobuf的RPC通信

XmlRpc++是一个基于C++的XML-RPC第三方库，也是ROS所使用的，需要头文件和链接动态库。我找了半天都没找到完整的Linux版本的，只好直接用ROS自带的，CMakeLists这样写：

1
2
3
4
5
include_directories(/opt/ros/kinetic/include/)
link_directories(/opt/ros/kinetic/lib)

add_executable(HelloServer "HelloServer.cpp")
target_link_libraries(HelloServer -lxmlrpcpp )		# 链接库文件

这个库其实很小，头文件如下：

ROS源码自带了一个XmlRpc++的测试程序，就是ros_comm\utilities\xmlrpcpp\test中的HelloClient.cpp和HelloServer.cpp，我们先看服务端

HelloServer

要想自定义一个远程调用，服务端需要实现一个抽象类XmlRpcServerMethod的子类，其内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
 // 类定义在命名空间 XmlRpc
// Representation of a parameter or result value
class XmlRpcValue;

//类XmlRpcServer处理客户端的请求以进行远程调用
class XmlRpcServer;
// 宏XMLRPCPP_DECL无定义，仅说明XmlRpc在ROS中做静态库编译
class XMLRPCPP_DECL XmlRpcServerMethod {
public:
  XmlRpcServerMethod(std::string const& name, XmlRpcServer* server = 0);
  virtual ~XmlRpcServerMethod();

  std::string& name() { return _name; }	// 远程调用的名称

  // 执行method，参数 XmlRpcValue，子类必须实现此纯虚函数
  virtual void execute(XmlRpcValue& params, XmlRpcValue& result) = 0;

  // 返回method的帮助信息，由子类实现
  virtual std::string help() { return std::string(); }

protected:
  std::string _name;		// method 名称赋值
  XmlRpcServer* _server;
};

看来这个类很简单，关键是method名称和实现纯虚函数，服务端程序如下：

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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#include "xmlrpcpp/XmlRpc.h"
#include <iostream>
#include <stdlib.h>
using namespace XmlRpc;

// The server
XmlRpcServer s;

// One argument is passed, result is "Hello, " + arg.
class HelloName : public XmlRpcServerMethod
{
public:		// HelloName是 method 名称
  HelloName(XmlRpcServer* s) : XmlRpcServerMethod("HelloName", s) {}
  void execute(XmlRpcValue& params, XmlRpcValue& result)
  {
    std::string resultString = "Hello, ";
    resultString += std::string(params[0]);
    result = resultString;		// 赋值给 result
  }
} helloName(&s);	// 类HelloName实例化

// A variable number of arguments are passed, all doubles, result is their sum.
class Sum : public XmlRpcServerMethod
{
public:
  Sum(XmlRpcServer* s) : XmlRpcServerMethod("Sum", s) {}
  void execute(XmlRpcValue& params, XmlRpcValue& result)
  {
    int nArgs = params.size();
    double sum = 0.0;
    for (int i=0; i<nArgs; ++i)
      sum += double(params[i]);
    result = sum;
  }
} sum(&s);

int main(int argc, char* argv[])
{
  if (argc != 2)
  {
    std::cerr << "Usage: HelloServer serverPort \n";
    return -1;
  }
  int port = atoi(argv[1]);
  // Specify the level of verbosity of informational messages. 0 is no output, 5 is very verbose.
  XmlRpc::setVerbosity(5);

  // 创建socket，绑定到端口，进入listen模式等待客户端
  s.bindAndListen(port);

  // Enable introspection
  s.enableIntrospection(true);

  // 等待一定时间以处理客户端请求，-1表示一直等待或指导exit()被调用
  s.work(-1.0);
  return 0;
}

服务端程序比较简单，主要是自定义method类后，使用XmlRpcServer的构造函数添加method，另外也可以用XmlRpcServer::addMethod(XmlRpcServerMethod* method);，构造函数中实际也是调用这个函数。

然后是函数setVerbosity:

1
2
3
// Easy API for log verbosity
int XmlRpc::getVerbosity() { return XmlRpcLogHandler::getVerbosity(); }
void XmlRpc::setVerbosity(int level) { XmlRpcLogHandler::setVerbosity(level); }

看来就是设置日志等级，不用再深究了。

bindAndListen其实就是封装了Linux网络编程的服务端常用函数，一看源码就清楚。至于work函数，内容如下：

1
2
3
4
5
6
// 在一定时间内处理客户端请求
void  XmlRpcServer::work(double msTime)
{
  XmlRpcUtil::log(2, "XmlRpcServer::work: waiting for a connection");
  _disp.work(msTime);
}

这里看到日志函数了，等级是2，这里的2要与上面设置的等级相比较，也就是XmlRpcLogHandler::getVerbosity()，如果不大于所设置等级才会有日志。然后是XmlRpcDispatch::work，这个就是通信的核心了， 看源码应该是poll的方式， 懒得深入研究了。

在终端输入./HelloServer 8888运行，开始监听端口8888，运行客户端后收到了HelloName的远程调用

然后服务端回复XML，内容基于Http协议



参考：
XmlRPC简介及使用

使用jsoncpp读写JSON|第三方库

还可以用另一个库 nlohmann

从Github上下载后，使用老三步安装：

1
2
3
4
mkdir dir && cd dir
cmake ..
make 
sudo make install

安装的路径如下：

在CMakeList里进行配置，别忘了最后链接库文件：-ljsoncpp

写JSON

1
2
3
4
5
6
7
Json::Value ret_json;

ret_json["msg"] = "Hello JSON";
ret_json["code"] = 123;
ret_json["ret"] = true;

cout << ret_json.toStyledString()<< endl;

运行结果：

1
2
3
4
5
{
   "code" : 123,
   "msg" : "Hello JSON",
   "ret" : true
}

写JSON数组如下：

1
2
3
4
5
6
7
8
9
10
Json::Value ret_json,sub;
sub.append(Json::Value(123));
sub.append(Json::Value(456));
sub.append(Json::Value(789));

ret_json["msg"] = "Hello JSON";
ret_json["code"] = sub;
ret_json["ret"] = true;

cout << ret_json.toStyledString()<< endl;

结果为：

1
2
3
4
5
{
   "code" : [ 123, 456, 789 ],
   "msg" : "Hello JSON",
   "ret" : true
}