之前博文《ROS学习笔记之——gmapping与amcl 》已经介绍过gmapping与amcl了。本博文详细的看一下amcl的代码。如需要修改amcl，用新的包，可以选择到/opt/ros/melodic/include/amcl目录下，把旧的amcl删掉，然后再编译新的，当然也可以通过更改名字（更改包的名字）。个人认为，更改名字是比较好的方法了哈～
目录
AMCL源码
TF
TF的数据类型
发布TF
订阅TF
关于TF的一些测试
参考资料
从之前博文以及实验中发现，amcl定位有两个关键点。
1，需要一个初始的姿态估计。初始的位姿估计并不需要很准确，大致的位置和orientation对了就可以了。
2，在移动的过程中，如果把amcl的点可视化出来，会发现随着机器人的移动，会逐渐收敛。（在之前博客中已经做过类似的仿真了，后来实验也可以得到类似的效果，但收敛的速度和最终收敛的大小，相对没有仿真的这么好）
amcl使用粒子滤波器在已知地图的情况下定位机器人位姿。它根据粒子集合的统计数据插入一定数量的新粒子，来解决全局定位失效或者说是机器人绑架的问题。 并根据KLD采样自适应的调节粒子数量，可以在定位精度和计算代价之间进行权衡。
AMCL源码
打开AMCL的源码，会发现有大量的文档，但是关键的节点应该就是amcl_node.cpp文件
关于源码的解读见下面注释
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=index#part6
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=amcl%E7%9A%84%E6%80%BB%E4%BD%93%E4%B8%9A%E5%8A%A1%E9%80%BB%E8%BE%91
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=amcl_demo%E4%B9%8B%E5%AE%9A%E4%BD%8D%E5%99%A8
通过turtlbot navigation中的amcl.launch文件定义了运行amcl的配置参数，
下面来看看amcl_node.cpp文件
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* Author: Brian Gerkey */
#include
#include
#include
TF
在amcl中，tf同样是非常重要的。
http://wiki.ros.org/tf/Overview
TF的数据类型
作为ROS中的特色，TF定义了它自己的数据类型
tf的消息类型为geometry_msgs/TransformStamped
std_mags/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w
发布TF
为了在一个节点中发布tf，需要采用tf::TransformBroadcaster
首先需要构建一个tf的发布者
tf::TransformBroadcaster();
然后发送
void sendTransform(const StampedTransform & transform);
void sendTransform(const geometry_msgs::TransformStamped & transform);
例程：下面代码将发布一个坐标帧到tf2上，随着turtles的运动，发布其坐标帧
1 #include
2 #include
3 #include //发布pose到tf2的头文件
4 #include
5 #include
6
7 std::string turtle_name;
8
9 void poseCallback(const turtlesim::PoseConstPtr& msg){
10 static tf2_ros::TransformBroadcaster br;//创建tf的发布者对象
11 geometry_msgs::TransformStamped transformStamped; //要发布的信息
//关于这个消息类型可以参考：http://docs.ros.org/en/jade/api/geometry_msgs/html/msg/TransformStamped.html
12 //往transformStamped里面加数据
13 transformStamped.header.stamp = ros::Time::now();//需要给要发布的tf一个timestamp（时间戳）
14 transformStamped.header.frame_id = "world";//设置父帧的名字
15 transformStamped.child_frame_id = turtle_name;//设置子帧的名字（此处设置为自己）
16 transformStamped.transform.translation.x = msg->x;
17 transformStamped.transform.translation.y = msg->y;
18 transformStamped.transform.translation.z = 0.0;
19 tf2::Quaternion q;//定义四元数
20 q.setRPY(0, 0, msg->theta);
21 transformStamped.transform.rotation.x = q.x();
22 transformStamped.transform.rotation.y = q.y();
23 transformStamped.transform.rotation.z = q.z();
24 transformStamped.transform.rotation.w = q.w();
25
26 br.sendTransform(transformStamped);//然后发送出去（包含了位置和方向）
27 }
28
29 int main(int argc, char** argv){
30 ros::init(argc, argv, "my_tf2_broadcaster");
31
32 ros::NodeHandle private_node("~");
33 if (! private_node.hasParam("turtle"))
34 {
35 if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
36 turtle_name = argv[1];
37 }
38 else
39 {
40 private_node.getParam("turtle", turtle_name);
41 }
42
43 ros::NodeHandle node;
44 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
45
46 ros::spin();
47 return 0;
48 };
参考：http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28C%2B%2B%29#CA-655844a832971675d8cca09ac2412a96f7a2c2c6_1
订阅TF
例程：
#include
#include //transform_listener就是接收tf的接收器
#include
#include
#include
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_listener");//初始化ROS节点
ros::NodeHandle node;//创建节点句柄
ros::service::waitForService("spawn");
ros::ServiceClient spawner =
node.serviceClient("spawn");
turtlesim::Spawn turtle;
turtle.request.x = 4;
turtle.request.y = 2;
turtle.request.theta = 0;
turtle.request.name = "turtle2";
spawner.call(turtle);
// 创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel =
node.advertise("turtle2/cmd_vel", 10);//发布者
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);//创建TransformListener的对象
//一旦tf的接收者创建成功，就会开始订阅tf
ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;//接收的tf样本
try{
获取turtle1与turtle2坐标系之间的tf数据
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
transformStamped.transform.translation.x);
vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
pow(transformStamped.transform.translation.y, 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
参考：http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29
关于TF的一些测试
我们先来看看正常开启机器人的tf树和开启amcl后的tf树有什么区别
正常开启机器人的时候，由turtlebot3_core发布了一个odom到base——footprint的tf
若由amcl进行定位。可以看到由amcl发布了一个由map到odom的
若由robot_localization进行定位
若同时发布两个回怎么样呢？
结果发现显示的东东跟上面一样。
单纯开启gmapping的时候的tf
参考资料
http://wiki.ros.org/tf/Overview
https://www.guyuehome.com/279
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=amcl%E7%9A%84%E6%80%BB%E4%BD%93%E4%B8%9A%E5%8A%A1%E9%80%BB%E8%BE%91
https://www.cnblogs.com/li-yao7758258/p/7672521.html
