(一) 概述,安装配置(避免找不到库文件),常用类

PCL包括多个子模块库。最重要的PCL模块库有如下:过滤器Filters、特征Features、关键点Keypoints、配准Registration、Kd树Kd-tree、八叉树Octree、切分Segmentation、Sample Consensus、Surface、Range Image、文件读写I/O、Visualization、通用库Common、Search
PCL框架.png

PCL中的所有模块和算法都是通过Boost共享指针来传送数据的,因而避免了多次复制系统中已存在的数据的需要

ROS中已经安装了pcl,不必再安装了,而且网上给的那个源一直添加失败,服务器在国外。但是ROS的pcl缺工具包,比如查看点云图片的pcl_viewer。它的安装命令是:sudo apt-get install pcl-tools

优点:

  • 可以表达物体的空间轮廓和具体位置

  • 点云本身和视角无关,也就是你可以任意旋转,可以从不同角度和方向观察一个点云,而且不同的点云只要在同一个坐标系下就可以直接融合

缺点:

  • 点云并不是稠密的表达,一般比较稀疏,放大了看,会看到一个个孤立的点。它的周围是黑的,也就是没有信息。所以在空间很多位置其实没有点云,这部分的信息是缺失的。

  • 点云的分辨率和离相机的距离有关。离近了看不清是个什么东西,只能拉的很远才能看个大概。

安装

编译pcl-1.8

1
2
3
4
5
6
mkdir build
cd build
# 配置cmake
cmake -DCMAKE_BUILD_TYPE=None -DCMAKE_INSTALL_PREFIX=/usr \ -DBUILD_GPU=ON-DBUILD_apps=ON -DBUILD_examples=ON \ -DCMAKE_INSTALL_PREFIX=/usr ..
make
sudo make install

如果ubuntu 22.04且通过sudo apt install libpcl-dev安装的pcl。那么pcl版本可能是1.12,如果vtk版本正好又是9.1,二者存在冲突会导致不能正常显示。

cmake配置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
cmake_minimum_required(VERSION 2.6)
project(pcl_test)

find_package(PCL 1.7 REQUIRED)
# 可选的工具包
find_package(catkin REQUIRED COMPONENTS
pcl_conversions
pcl_ros
)
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_test main.cpp)
target_link_libraries (pcl_test ${PCL_LIBRARIES})

PCL中常见的PointT类型

PointXYZ是使用最常见的一个点数据类型,因为他之包含三维XYZ坐标信息,这三个浮点数附加一个浮点数来满足存储对齐,可以通过points[i].data[0]或者points[i].x访问点X的坐标值

1
2
3
4
5
6
7
8
9
10
union
{
float data[4];
struct
{
float x;
float y;
float z;
};
};

PointXYZRGB也是个union,成员如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
union{
float data[4];
struct
{
float x;
float y;
float z;
};
};
union{
struct{
float rgb;
};
float data_c[4];
};

在使用PCL库的时候,经常需要显示点云,可以用下面这段代码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
#include <pcl/visualization/cloud_viewer.h>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

PointCloudT::Ptr cloud(new PointCloudT);

cloud->is_dense = false;
// 打开点云文件,获取的是vertex
if(pcl::io::loadPLYFile("file.ply", *cloud) <0 )
{
PCL_ERROR("loading cloud failed !");
return -1;
}
cout<< "点云共有: "<< cloud->size()<<"个点"<<endl; //文件中vertex的个数

pcl::visualization::CloudViewer viewer ("Viewer");
viewer.showCloud(cloud);

while (!viewer.wasStopped ())
{

}

变换点云

1
pcl::transformPointCloud (*cloud_in, *cloud_out, matrix);

cloud_in为源点云,cloud_out为变换后的点云,注意这里的matrix是4X4的欧拉矩阵,也就是第一行为R,t;第二行为0,1的矩阵

PCLVisualizer类与CloudViewer类

对于CloudViewer类来说,其方法有大致以下几类

1
2
3
4
5
6
7
8
9
10
11
12
void showCloud()

void wasStopped() // 关闭窗口

void runOnVisualizationThread()
void runOnVisualizationThreadOnce ()

void removeVisualizationCallable()
// 键盘鼠标的响应
boost::signals2::connection registerKeyboardCallback()
boost::signals2::connection registerMouseCallback()
boost::signals2::connection registerPointPickingCallback()

常用代码:

1
2
3
4
5
6
pcl::visualization::CloudViewer viewer("3D Point Cloud Viewer");
// 除了显示什么都不能干
viewer.showCloud(m_cloud);
while(!viewer.wasStopped())
{
}


如果只是简单显示点云,调用CloudViewer就可以了。PCLVisualizer更加强大,可以改变窗口背景,还可以添加球体,设置文本,功能特别丰富

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);

int v1(0);
int v1(1);
// 视窗的x轴最小值,y轴最小值,x轴最大值,y轴最大值
viewer->createViewPort(0.0, 0, 0.5, 1.0, v1); //创建左视窗
viewer->createViewPort(0.5, 0, 1.0, 1.0, v2); //创建右视窗
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
// 代码最后,都会出现这样一个while循环,在spinOnce中为界面刷新保留了时间
while (!viewer->wasStopped())
{
viewer->spinOnce(100); // 多长时间调用内部渲染一次
boost::this_thread::sleep(boost::posix_time::microseconds(100000)); //延时0.1秒
}

addPointCloud函数将点云添加到视窗对象中,并定义一个唯一的字符串作为ID号,利用此字符串保证在其他成员方法中也能标识引用该点云,多次调用addPointCloud(),可以实现多个点云的添加,每调用一次就创建一个新的ID号。如果想更新一个已经显示的点云,可以使用updatePointCloud函数。删除有removePointCloud(ID号)removeAllPointClouds。 这里使用的是最简单的一个重载,此外可以在第二个参数添加PointCloudColorHandlerPointCloudColorHandler

PCLVisualizer类的键盘事件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
bool next_iteration = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing){
if(event.getKeySym() == "space" && event.keyDown()){
next_iteration = true;
}
}

int main()
{
pcl::visualization::PCLVisualizer *viewer;
// 必须要给PCLVisualizer指针赋值实例化对象
viewer = new pcl::visualization::PCLVisualizer("PCL Windows");
viewer->registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

while(!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds (100000));
if(next_iteration)
{}
}
return 0;
}

其他常用函数

1
2
3
4
5
6
7
8
// 初始化相机参数
initCameraParameters();
setCameraPosition(
0, 0, 5, // camera位置
0, 0, 4, // view向量--相机朝向
0, 1, 5 // up向量
);

还可以对不同的视窗添加文字和背景颜色

参考:
PCL库简要说明
PCL中可用的PointT类型
PCLVisualizer
PCLVisualizer可视化类


(一) 安装和常用操作

运行博物馆数据集

launch修改如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
<launch>
<param name="/use_sim_time" value="false" />

<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />

<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>

<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

一个终端先运行launch,再到另一个终端运行rosparam set use_sim_time true,再播放cartographer_paper_deutsches_museum.bag,否则会有时间戳问题

如果只运行bag文件是没有tf信息的,其实是在urdf里面:
德意志博物馆的tf树.png

使用rosbag最好不要加倍播放,很可能出现tf的时间戳问题,可能是lookup时间导致

注意rviz需要有这三项

我的rviz插件的名称和官方不同,保证话题一样就行:

使用雷达online建图

我先拿思岚A2雷达测试,运行雷达的命令是:roslaunch rplidar_ros rplidar.launch,参见ROS中使用激光雷达(RPLIDAR)

复制/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files/revo_lds_.lua文件为no_odom_and_imu.lua,在它基础上修改22和23行:

1
2
tracking_frame = "base_footprint",   --原来是laser
published_frame = "base_footprint", --原来是laser

复制catkin_ws/src/cartographer_ros/cartographer_ros/launch/demo_revo_lds.launchhome_map_no_odom.launch,在它基础上修改23行: -configuration_basename no_odom_and_imu.lua",最终是这样:
1
2
3
4
5
6
7
8
9
10
<launch>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename no_odom_and_imu.lua"
output="screen">
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

现在跟普通的launch文件不一样了,我们需要的是/home/user/catkin_ws/install_isolated/share/cartographer_ros中的launch和lua,此时还没有,可以直接复制过去或者重新编译:

1
2
cd ~/catkin_ws
catkin_make_isolated --install --use-ninja

添加source install_isolated/setup.zsh到 bash.rc,否则执行 roslaunch 会找不到文件。

现在运行roslaunch cartographer_ros home_map_no_odom.launch,效果如下:
小强机器人+cartographer
启动后,不移动时,正常的建图日志

1
2
3
4
5
6
7
8
9
10
11
12
Extrapolator is still initializing.
[pose_graph_2d.cc:148] Inserted submap (0, 0).

[pose_graph_2d.cc:538] Remaining work items in queue: 0
[constraint_builder_2d.cc:284] 0 computations resulted in 0 additional constraints.
[constraint_builder_2d.cc:286] Score histogram: Count: 0

[motion_filter.cc:42] Motion filter reduced the number of nodes to 4.2%.
[pose_graph_2d.cc:538] Remaining work items in queue: 0
[constraint_builder_2d.cc:284] 0 computations resulted in 0 additional constraints.
[constraint_builder_2d.cc:286] Score histogram: Count: 0
scan rate: 13.27 Hz 7.54e-02 s +/- 2.37e-03 s (pulsed at 100.01% real time)

节点关系

cartographer_node是各个节点或话题的枢纽。它订阅话题scan,有时会有imu(惯导数据),然后完成定位与子图的构建,并发布话题 submap_listsubmap_list这个话题被/cartographer_occupancy_grid_node订阅,用于构建子图占用的栅格地图。

保存地图

首先不再接受进一步数据

1
rosservice call /finish_trajectory 0

rviz的建图画面会冻结,service的response如下:
1
2
3
status:
code: 0
message: "Finished trajectory 0."

日志如下:
1
2
3
4
5
Shutdown the subscriber of [scan]
[ map_builder_bridge.cc:152] Finishing trajectory with ID '0'...
[pose_graph_2d.cc:538] Remaining work items in queue: 0
[constraint_builder_2d.cc:284] 0 computations resulted in 0 additional constraints.
[constraint_builder_2d.cc:286] Score histogram: Count: 0

这是结束轨迹,进行最终优化

然后序列化保存其当前状态:

1
rosservice call /write_state "{filename: '${MAPS}/home.pbstream'}"

pbstream文件是个二进制文件,包含已经保存的SLAM状态,可以在启动cartographer_node时加载

再将pbstream 转换为pgm和yaml

1
rosrun cartographer_ros cartographer_pbstream_to_ros_map  -map_filestem=${MAPS}/home  -pbstream_filename=${MAPS}/home.pbstream -resolution=0.05

然后出现:
1
2
[pbstream_to_ros_map_main.cc:50] Loading submap slices from serialized data.
[pbstream_to_ros_map_main.cc:70] Generating combined map image from submap slices.

生成pgm和yaml文件

其实可以不结束轨迹直接保存地图

报错

正式使用时,我在机器人工控机装好了 cartographer,在本地通过SSH启动,结果出现了报错:submapslist话题的md5值不同

2020-03-25_104035.png

建出来的图是这样的:
建图出错,未形成面

这是因为本机的rviz缺了一个插件,在编译carto时会装到rviz上,这就是一个棘手的问题,如果要在本机上开启rviz,就要在本机也把cartographer这一套编译通过,也就是需要装两次。如果本机安装不成功,还可以使用vncserver在远程机上启动rviz
cmake中的部分
rviz_plugin_description

插件效果
rviz加载submap插件失败

参考:
安装过程


命名空间

命名空间的三级参数如何设置

参数 /robot_pose 的设置

1
2
ros::NodeHandle  nh;
nh.getParam("robot_pose", robot_pose_ );

/move_base/controller_frequency的设置

1
2
3
4
// 在 move_base 节点内
ros::NodeHandle n("~/");
double control_frequency;
n.param("controller_frequency", control_frequency, 15.0);

参数 /move_base/Planner/linear_vel 的设置

1
2
3
// 在 move_base 节点内
ros::NodeHandle nh("~/" + string("Planner") );
nh.param("linear_vel", linear_vel_, 0.3);


1
2
3
4
5
ros::init(argc, argv, "my_node_name");
ros::NodeHandle nh1("~");
ros::NodeHandle nh2("~foo");
ros::Subscriber sub1 = nh1.subscribe("topic1", ...);
ros::Subscriber sub2 = nh2.subscribe("topic2", ...);

平时对nh,是用无参的构造函数,此时对应的话题是topic1。 加上~后,sub1订阅的话题是my_node_name/topic1。如果不是~,而是word,话题是/word/topic1

对于sub2,命名空间是~foo,相当于上面两种的组合,订阅的话题是my_node_name/foo/topic2

当然可以在launch文件修改命名空间的第一级,也就是 <node pkg="package", name="my_node", type="my_bin" />


cmake教程(六)add_dependencies规定编译依赖项

ROS的工作空间用的时间一长,就会创建很多package,有些package的编译又用到了其他的package,这时单纯使用catkin_make就会出现问题。比如package A用到了B,B里又用到了C。以往使用catkin_make --pkg编译,但是可能会需要把所有的package都处理一遍,此时处理到A时就会报错,也就无法编译B和C。比较笨的方法就是在A的CMakeLists里先把B和C注释掉,同样在B的CMakeLists里把A注释掉,先用catkin_make --pkg C编译C,再依次编译B和A。

经过研究发现使用add_dependencies就可以解决这个问题,但是不能解决find_package的问题

在A的CMakeLists里这样写:

1
2
3
4
5
find_package(catkin REQUIRED COMPONENTS B C)

add_executable(A_program src/A_program.cpp)
add_dependencies(A_program B C ) # B C是package名称,不是编译结果的名称
target_link_libraries(A_program ${catkin_LIBRARIES} B_program C_program)

add_dependencies必须在add_executable之后,可以在target_link_libraries之前或之后。 这样在编译A的时候,会出现一句scanning dependencies,这时编译器就会注意到A的依赖项,等依赖项完成后再编译A。同样在B的CMakeLists里添加C作为依赖项。



有些package需要依赖自定义的消息或服务文件,也就是一个特殊的package,这就更简单了,有专门的命令: add_dependencies(your_program ${catkin_EXPORTED_TARGETS}),不必用专门的package名称,用message函数可以看这个宏的内容,它是一大堆消息和服务名称。这样在编译的时候会出现这样的信息:

现在编译就快捷多了,直接用catkin_make就可以编译整个工作空间,不必自己改编译顺序了。


激光SLAM综述

机器人依据自由度的可控性(controllable)划分为nonholonomic或者holonomic robot

  • holonomic robot: 在所有的自由度上都可控的,比如基于万向轮的机器人(robot built on Omni-wheels)

  • nonholonomic robot: 差速车或阿克曼车。可控的自由度维数小于机器人自身的自由度,比如car自身有3个自由度(x,y,theta),即平面坐标和车身朝向,但car可控的自由度是2(加减速度,转动方向盘,不能随意的漂移运动)

从另一个直观通俗的角度可以理解为:如果机器人可以在N维空间中朝任意方向移动,则对于N维空间是完整性的。机器人在N维空间中不能朝任意方向移动,则认为是非完整性约束。

2D激光SLAM所有流派

传感器

IMU:

  • 直接测量角速度和线加速度
  • 角速度的测量精度高

轮式里程计:

  • 直接测量机器人的位移和角度
  • 较高的局部角度和位置测量精度
  • 更新速度高 (100Hz~200Hz)

传感器辅助(odom+IMU)

  • 极高的位姿更新频率,可以比较准确反应运动情况
  • 高精度的局部位姿估计
  • 跟状态估计解耦,所有信息都来自传感器,不需要匹配


数据预处理:

  • 轮式里程计的标定:轮式里程计的机械标称值并不代表真实值,实际误差可能较大
  • 激光雷达运动畸变去除:每一帧激光数据的采集需要时间,在采集期间如果机器人运动会使测量值产生畸变。运动产生的畸变会让数据严重失真,影响匹配精度。
  • 不同系统之间时间同步(不同CPU)

实际环境中的问题:

  • 大尺度的动态环境,即有高动态(行走的人和移动前后的物体)
  • 环境变化

  • 几何结构相似的环境,比如长走廊

  • 全局定位:信息量小

  • 地面材质的变化

  • 地面凹凸不平
  • 机器人载重改变(与前两条一起影响里程计)

后面几条为传感器融合可以解决的问题

激光slam的问题:

  • 退化环境: 对于2D激光雷达来说是hallway走廊; 对于3D激光雷达来说,很空旷的环境
  • 地图的动态更新
  • 全局地位
  • 动态环境定位:动态物体检测与跟踪解决

实际问题

  • 环境中的透明玻璃和网状结构,怎么解决
    玻璃也有一定的反射率,大约10%。建图之后,可以直接把玻璃漏光部分直接编辑为黑线。

  • 多线激光常见方案:NDT-mapping和LOAM。3D激光SLAM一般与视觉融合,这是常用方法。LOAM不适合2D激光

  • 主流的激光SLAM可以在普通ARM CPU上实时运行。激光雷达主动发射,在较多机器人时可能产生干扰。尤其是固态激光雷达的大量使用,可能使得场景中充满了信号污染,从而影响激光SLAM的效果。

  • 非结构化环境:不存在特定的平面,比如马路(特定的平面和标志)。

  • 开发农作物网面的识别和采集地图,需要用激光雷达好还是用3D摄像头来开发(采集环境信息然后写识别算法去识别需要找的农作物目的)好?

因为农作物的环境比较复杂,准确度要求也比较高,所以使用Realsense,16线雷达的数据不够,32线又太贵了

机器人绑架问题

机器人定位分为三大问题:全局定位位姿跟踪绑架劫持

  • 全局定位:初始位置未知,机器人靠自身运动确定自己在地图中的位姿。

  • 位姿跟踪:已知自身位姿或者已经通过全局定位得到了一个较好的位姿估计初值,在后续运动时补偿精度较差的运动控制误差

  • 绑架劫持:机器人在已知自身位姿的情况下,得到了一个错误的位姿或者外界将其放到另外一个位姿,而里程计信息给出了错误的信息甚至没有给出控制信息。

最典型的粒子就是人把机器人搬走,机器人就找不到自己位置。这时用激光和里程计都不起作用了,可以用视觉辅助定位。用摄像头建一个特征地图,视觉SLAM和激光SLAM并行使用,激光SLAM失效时换视觉。比如使用二维码全局定位。

另外在绑架发生之前,保存当前状态,则可以使用除视觉传感器之外的传感器,比如反光板


move_base分析(四) planThread线程

MoveBase::planThread()

三个函数唤醒了planThread线程: MoveBase::wakePlanner MoveBase::executeCb MoveBase::executeCycle

MoveBase::planThread()

第一部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false; // 先不唤醒线程
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
while(n.ok() )
{
// check if we should run the planner (the mutex is locked)
while(wait_for_wake || !runPlanner_){
//if we should not be running the planner then suspend this thread
log.debug("[move_base_plan_thread] %s","Planner thread is suspending");
planner_cond_.wait(lock);
wait_for_wake = false;
}
// 开启线程
ros::Time start_time = ros::Time::now();
//time to plan! get a copy of the goal and unlock the mutex
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread", "Planning...");
// run planner
planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
......
}

前面就是定义一些变量和unique_lock,然后进入一个大while循环,嵌套一个while循环,由于runPlanner_在构造函数里声明为false,所以条件为真,直接锁住了这个线程,wait_for_wake仍为false

move_base还未完全启动时,线程planThread就开始运行了,然后才会从地图数据加载代价地图。所以在启动时,不要在线程里访问一些未初始化的变量。

线程一直运行在while循环

1
2
3
while(wait_for_wake || !runPlanner_ )
{
}

我在move_base里定义了一个发布者pub,如果在这个while循环里执行pub.publish(msg)move_base就会出错退出。可以使用智能指针,其实Publisher该有个函数,比如isValid来验证是否可用。我发现用getTopic()函数也可以,如果结果不是空,说明发布者和roscore的通信成功了,此时可以调用publish函数了。

在上面的三个函数唤醒线程后,就往下运行,获得临时目标点,clear planner后,进入makePlan

第二部分

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
if(gotPlan)
{
ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
//pointer swap the plans under mutex (the controller will pull from latest_plan_)
std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;

lock.lock();
// 交换变量,赋值给 latest_plan_
planner_plan_ = latest_plan_;
latest_plan_ = temp_plan;
last_valid_plan_ = ros::Time::now();
planning_retries_ = 0;
new_global_plan_ = true;

//make sure we only start the controller if we still haven't reached the goal
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
// 如果没找到路径,而且还在 PLANNING 状态(机器人不动)
else if(state_==PLANNING)
{
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
//check if make a plan for over time limit or maximum number of retries
lock.lock();
// lock是用于这个非原子操作
planning_retries_++;
// 如果参数 max_planning_retries还是默认值 -1,那转为uint32_t就是一个很大的数,就是不考虑重试次数
if(runPlanner_ &&
(ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)) )
{
// 进入 CLEARING 状态
state_ = CLEARING;
runPlanner_ = false; // proper solution for issue #523
publishZeroVelocity(); // 车不准动
recovery_trigger_ = PLANNING_R;
}
lock.unlock();
}
//if can't get new plan, robot will move with last global plan
// 其实跟上面的情况一样
else if(state_ == CONTROLLING)
{
movebase_log.warn("Robot in controlling, but can't get new global plan ");
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = PLANNING_R;
}
//take the mutex for the next iteration
lock.lock();
if(planner_frequency_ > 0)
{
ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
if (sleep_time > ros::Duration(0.0) )
{
wait_for_wake = true;
// wakePlanner的函数只有一句: planner_cond_.notify_one();
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}

最后的部分就是根据全局路径规划频率,调用makePlan的地方。


move_base分析(三)源码和调用流程

move_base_node.cpp

先来看move_base_node.cpp的源码,我们实际需要的程序是这个:

1
2
3
4
5
6
7
8
ros::init(argc, argv, "move_base_node");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug);

tf::TransformListener tf(ros::Duration(10));
move_base::MoveBase move_base( tf );
ros::spin();

return(0);

MoveBase类

MoveBase构造函数

MoveBase构造函数开始对一些成员进行了列表初始化,没有值得说明的

第一部分

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
//  枚举变量RecoveryTrigger,可取值有PLANNING_R, CONTROLLING_R, OSCILLATION_R
recovery_trigger_ = PLANNING_R;

// as_维护movebase的actionServer状态机,并且新建了一个executeCb线程
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
// 给一系列参数设置默认值
std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("/map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1);
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

// set up plan triple buffer, 声明3个vector指针
// 三个plan都是global_plan,最终由controller_plan_ 传给local planner: if(!tc_->setPlan(*controller_plan_))
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

第二部分

1
2
3
4
5
6
7
8
9
10
11
12
13
//建立planner线程
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

//用于控制机器人底座
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);

// we'll provide a mechanism for some people to send goals as //PoseStamped messages over a topic, they won't get any useful //information back about its status, but this is useful for tools like rviz
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

先是运行了一个线程planner_thread_,详细看MoveBase::planThread()。接下来一共涉及了4个ros::NodeHandle对象,注册了3个话题,订阅了话题goal,订阅话题的回调函数是MoveBase::goalCB

第三部分

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
// /机器人几何尺寸有关参数  we'll assume the radius of the robot to be consistent with what's specified for the costmaps
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

//创建planner's costmap的ROS封装类, 初始化一个指针,用于underlying map
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

//初始化global planner
try {
// boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
// 模板类ClassLoader,实例化 BaseGlobalPlanner
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
log.info("Created global_planner %s", global_planner.c_str());
} catch (const pluginlib::PluginlibException& ex) {
log.fatal("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}

//创建controller's costmap的ROS封装类, 初始化一个指针,用于underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

//初始化local planner
try {
tc_ = blp_loader_.createInstance(local_planner);
log.info("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
log.fatal("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
// 开启costmap基于传感器数据的更新
planner_costmap_ros_->start();
controller_costmap_ros_->start();

//advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);

//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
// if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
log.debug("[move_base] %s","Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}

planner_costmap_ros_ 用于全局导航的地图
controller_costmap_ros_ 用于局部导航的地图

GlobalPlanner类,继承nav_core::BaseGlobalPlanner, Provides a ROS wrapper for the global_planner planner which runs a fast, interpolated navigation function on a costmap.

第四部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// 载入recovery behavior, 这部分是状态机的设计问题,优先加载用户的设置,如果没有则加载默认设置
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
//initially, we'll need to make a plan
state_ = PLANNING;

//we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;

// start the action server
as_->start();

dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);

MoveBase::executeCb

每次有新的goal到来都会执行此回调函数, 如果有新目标到来而被抢占则唤醒planThread线程, 否则为取消目标并挂起处理线程

MoveBase::executeCycle

local planner的核心工作函数, 只在MoveBase::executeCb中调用一次,它是局部路径规划的工作函数,其中会调用computeVelocityCommands来规划出当前机器人的速度


局部路径规划
1
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));

local planner

根据附近的障碍物进行躲避路线规划,只处理局部代价地图中的数据. 是利用base_local_planner包实现的。该包使用Trajectory RolloutDynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度。

对于全向机器人来说,也就是存在x方向的速度,y方向的速度,和角速度。DWAlocalplanner确实效率高一点。但是如果是非全向机器人,比如说只存在角速度和线速度,trajectorylocalplanner会更适用一点。

base_local_planner这个包通过地图数据,通过算法搜索到达目标的多条路经,利用一些评价标准(是否会撞击障碍物,所需要的时间等等)选取最优的路径,并且计算所需要的实时速度和角度。
其中,Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下:

  1. 采样机器人当前的状态(dx,dy,dtheta);
  2. 针对每个采样的速度,计算机器人以该速度行驶一段时间后的状态,得出一条行驶的路线。
  3. 利用一些评价标准为多条路线打分。
  4. 根据打分,选择最优路径。
  5. 重复上面过程。



发送导航目标点后,首先在rviz上出现一个绿色全局路径,机器人开始行走,同时还有一个蓝色的本地路径,只是很小,不容易观察到,示意图如下:

现在机器人正在按照规划的全局路径行走,地图中突然增加了一个障碍物,它不会进入全局代价地图,但会在局部代价地图中创建.所以全局路径开始没有改变,当它快接近障碍物时,局部路径检测到了它,进而会重新规划一个全局路径,这样避开了障碍物. 局部规划器把全局路径分成几部分,再执行每一部分,生成控制命令。

话题

机器人行走时,在rviz上显示全局路径的相关话题是/move_base/DWAPlannerROS/global_plan,属于Global Map。局部规划器把全局路径的一部分发布到这个话题,代码是在DWAPlannerROS::computeVelocityCommands——DWAPlannerROS::publishGlobalPlan

rviz中显示局部路径的相关话题是/move_base/DWAPlannerROS/local_plan,属于Local Map。一旦局部路径计算出来,它就会发布到这个话题,代码是在DWAPlannerROS::computeVelocityCommands——DWAPlannerROS::publishLocalPlan

常用算法

ROS的局部规划器的接口plugin类为nav_core::BaseLocalPlanner,常用的包有4种:

  • base_local_planner - 提供了 Dynamic Window Approach(DWA)和Trajectory Rollout方法来局部控制;
  • dwa_local_planner - 模块化DWA方法应用,相比base_local_planner具有更多的恢复机制、更便于理解的接口和针对全向运动机器人更灵活的Y轴变量控制;
  • teb_local_planner

参考:
dwa_local_planner


使用机器人建图的技巧

可以参考
如何在环境轮廓建图完成之后,继续完善地图细节
SLAMTEC激光建图传感器使用技巧

建地图时,最好雷达面对墙壁,小车逐渐倒退来完善地图,不要让小车正面对着墙壁和空旷环境前进,在某部分环境建地图后,可以让小车正面前进。

绘制的地图,黑线(墙)周围的黑点越少越好,毛刺越少越好,漏光线的环境会出现很长的白线,蓝色的块是避障保护区。

建图时出现重影


一般有两个方面的原因:

  1. 里程计累计误差
  2. 建图数据的时间戳可能没有对齐。

一般第一种比较常见,比如说编码器是否存在丢失脉冲的情况、换个比较坚硬的地面跑一跑看看建图效果。第二种比较难排查,最好能够录下数据查看。


sed和awk

删除文本第四行

1
sed '4d' input.txt > output.txt

实际上没有修改input.txt,而是修改文件流,然后生成到文件output.txt。前后两个文件名称不能相同,否则不能保存

使用sed处理当前目录(包括子目录)的所有cpp文件,将其中的aaa替换为bbb

1
sed -i "s/aaa/bbb/g" `grep aaa -rl . --include="*.cpp" `

grep:

  • r 表示查找所有子目录
  • l 表示仅列出符合条件的文件名,用来传给sed命令做操作
  • --include="*.cpp" 表示仅查找cpp文件

整个grep命令用`号括起来,表示结果是一系列文件名

sed -i后面是要处理的文件,s/aaa/bbb/g跟vim下的替换命令一样,表示把aaa全替换为bbb,加g表示一行有多个aaa时,要全部替换,

删除空白行: sed ‘/^$/d’ -i delete.txt

对于当前目录(包括子目录)的所有txt文件,删除含有以ros开头字符串的所有行

1
sed -i '/ros/d' `grep -rl "^ros" . --include="*.txt" `

删除前5个字符

1
sed 's/.\{5\}//' test.log > a.log