地图

代价地图的参数定义在三个文件里，分别用于全局代价地图，局部代价地图，通用代价地图(用于两个代价地图的参数)

movebase建立了两个costmap，其中`planner_costmap_ros是用于全局导航的地图，controllercostmap_ros`是用于局部导航用的地图。

通用代价地图

参数文件common_costmap_parameters.yaml，这些参数会影响两个代价地图:

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

footprint: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]]
inflation_radius: 0.05  #before: 1.8  0.3  1.2
obstacle_range: 2.5
raytrace_range: 3.0
cost_scaling_factor: 0.18
observation_sources: scan
scan:
  data_type: LaserScan
  topic: scan
  marking: true
  clearing: true

map_type: costmap

• footprint: 机器人底盘的轮廓，由一系列二维数据表示，单位为米，每个表示机器人边界的点，逆时针或顺时针方向都可以，轮廓一般是个矩形，机器人中心相当于[0,0]。本参数其实用于表示内切圆和外接圆，它们以一定方式膨胀障碍物以适应机器人。一般为安全起见，本参数比真实轮廓稍微大一些。

• robot_radius: 与footprint并列，用于圆形底盘，这个设置就简单多了，就是个半径值，单位米

• min_obstacle_height: 传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。

• obstacle_range: 设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物

• raytrace_range: 在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间

全局代价地图

全局地图是从静态地图创建的，使用了后者的数据，只是全局地图将静态地图中的障碍物膨胀化, 依据机器人的足迹半径.

相应的参数文件global_costmap_params.yaml：

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

global_costmap:
  global_frame: map
  robot_base_frame: base_link    # 或者 base_footprint
  update_frequency: 2.0     #before: 5.0
  publish_frequency: 2.0      #before 0.5
  static_map: true
  rolling_window: false     # 与static_map相反
  transform_tolerance: 0.5  # tf变换允许的最大时间

plugins:
  - {name: static, type: "costmap_2d::StaticLayer"}
  - {name: obstacles, type: "costmap_2d::VoxelLayer"}
  - {name: inflation, type: "costmap_2d::InflationLayer"}

plugins表示你要在costmap加载哪几层，static,obstacles,inflation是不可缺少的。 这些层在common_costmap_parameters.yaml中定义，然后添加到全局和局部代价地图的yaml文件里。如果没有定义，就不能添加到后两者。

全局代价地图对应的话题是/move_base/global_costmap/costmap，注意类型是nav_msgs/OccupancyGrid

加载代价地图后，可以在Rviz里看到基本参数信息

记录一个bug

全局代价地图如果正常生成，理应和地图的大小一样，是2000 x 2000

Costmap2D::worldToMap在刚启动时的打印结果

发导航命令后，代价地图的大小很奇怪地变为200, 原点变为(0,0)

1
2
3
4
5
6
7
8
9
10
11

Costmap2DR0S transform timeout. Current time: 1687663324.0445, global_pose stamp 321.6492, tolerance: 0.3000
[1687663324.0756730651]: Could not get robot pose, cancelling reconfiguration
[1687663324.6495552631]: MoveBase received move_base goal (48.000000, 58.000000), type: 3
[1687663324]: wx: 46.032079, wy: 46.095481, origin.x: 0.000000, origin_y_: 0.000000

[1687663324]: mx: 920, my: 921 , size_x_: 200, size_y_: 200, resolution.: 0.050000

[1687663324]: wx: 46.032079, wy: 46.095481, start_x_i: 920, start_y_i: 921
[1687663324]: The robot's start position is off the global costmap. Planning will always fail, you sure the robot has been properly localized ?
[1687663324.]: move_base failed to find a plan to point (48.00, 58.00) !
[1687663324.]: wx: 46.032090, wy: 46.095525, start_x_i: 920, start_y_i: 921

局部代价地图

局部规划器用局部代价地图计算出局部路径，局部代价地图是依据机器人的传感器数据创建的，与静态地图无关，它由用户给出宽度和高度。不管机器人怎么运动，它总在局部代价地图的中心，不再这个动态窗口内的障碍信息会drop

相应的参数文件local_costmap_params.yaml：

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

local_costmap:
  global_frame: odom       # 局部代价地图运行的坐标系，也可以是map
  robot_base_frame: base_link   # 也可以是map
  update_frequency: 5.0     # 局部代价地图的更新频率，太大会影响消耗CPU
  publish_frequency: 5.0    # 局部代价地图更新的发布频率，如果机器人运动很快，需要增大
  static_map: false      # 只能是false
  rolling_window: true  # 只能是true，代价地图在一个rolling window内更新
  width: 4     # rolling window的宽度，在rviz显示时会四舍五入
  height: 4    # rolling window的高度，在rviz显示时会四舍五入
  resolution: 0.02    # rolling window的分辨率
  transform_tolerance: 0.5   # tf变换的最大延迟

  plugins:
  - {name: obstacles_layer,  type: "costmap_2d::ObstacleLayer"}
  - {name: obstacles,    type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

加载局部代价地图成功后

局部代价地图的origin_x_和origin_y_(在costmap_2d.cpp中的成员变量)如图解释，显然随着机器人的运动会变化。

局部代价地图有一个更新的过程：

1. 订阅传感器相关主题，根据接受的传感器数据更新代价地图
2. 执行标记和清除动作，前者是向代价地图insert障碍信息，后者是从代价地图remove障碍信息。两种动作在障碍层中定义
3. 计算每个网格的代价值
4. 对每个包含障碍的cell进行障碍膨胀，从每个占据的cell按一定的膨胀半径向外扩张代价值

两个代价地图的设置中，膨胀层要放到所有层的最后，否则全局路径不是贴着膨胀层，而是障碍层。

Rviz的map包含3个Color Scheme，costmap, map, raw。若要增加主题，只能写一个Rviz插件或者修改Rviz的源码

参考：代价地图全面分析总结

使用catkin_make编译时，有时编译内容太长,而错误在终端的上面，使用滑条仍有可能看不到。这时可以使用catkin_make > path/file.log将编译内容输出到某个文件，这样就容易查看了。 catkin本身不会存储编译过程的日志

catkin_package()

这个函数十分重要，观察CMakeLists就可以发现，它是唯一的非cmake函数，显然ROS的catkin_make就是靠它了，它的作用是指定功能包构建信息导出，在使用add_library()或者add_executable() 之前，必须调用此函数。

该函数有5个可选参数：

1
2
3
4
5
6
7
8
9

INCLUDE_DIRS  导出包含功能包的路径(即cflags)

LIBRARIES 项目中导出的库

CATKIN_DEPENDS  该项目所依赖的其他catkin项目

DEPENDS 此项目依赖的非catkin CMake项目

CFG_EXTRAS  其他配置选项

1
2
3
4
5

catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
   CATKIN_DEPENDS roscpp nodelet
   DEPENDS eigen opencv)

catkin_package在/opt/ros/kinetic/share/catkin/cmake/catkin_package.cmake文件中定义。devel和build文件夹内的内容基本都是由其生成的。

这个函数解析package.xml文件，提取出里面的参数，执行catkin_make时，会在workspace/devel/share里生成pkgConfig.cmake文件的，在cmake文件中find_package就会用到这些文件，这样其他的对这个软件包有依赖的程序就可以方便的使用了。

比如现在有sub这个包，编译之后会在devel/share/sub/cmake生成两个文件：pubConfig.cmake和pubConfig-version.cmake，然后到另一个包pub的CMakeLists里调用sub：find_package(sub)，这样编译pub就通过了．假如把那两个文件删掉，然后使用catkin_make --pkg pub编译，就会报错：

对于build文件夹，生成是一些编译中的中间文件，比如用来存储一些环境变量之类的文件。这个文件夹意义不大。

在工程中显示其他源文件

新建CMake工程后，只有main.cpp，向目录添加文件foo.h和foo.cpp，但是它们不会在工程目录里显示，编辑CMakeLists如下

1
2
3
4
5
6
7
8

cmake_minimum_required(VERSION 2.8)

project(testStream)

include_directories(.)

# 或者add_library(Foo "foo.cpp")
add_executable(${PROJECT_NAME} "main.cpp" "foo.cpp")

报错Policy CMP0046 is not set

可以不用管，也可以按提示修改，最好先安装：sudo apt-get install libproj-dev

参考：Policy CMP0046问题

Memcheck

默认的工具，用来检测程序中出现的内存问题，所有对内存的读写都会被检测到，一切对malloc、free、new、delete的调用都会被捕获。所以，它能检测以下问题：

1. 对未初始化内存的使用；

2. 读/写释放后的内存块；

3. 读/写超出malloc分配的内存块；

4. 读/写不适当的栈中内存块；

5. 内存泄漏，指向一块内存的指针永远丢失；

6. 不正确的malloc/free或new/delete匹配；

7. memcpy()相关函数中的dst和src指针重叠

valgrind只报告内存泄漏，但没有显示具体代码中泄漏的地方。因此需要使用--leak-check=full选项启动valgrind。你的程序将比正常的运行慢得多（如20〜30倍），并使用更多的内存。Memcheck会报告关于内存错误，和检测到的泄漏的消息。

Memcheck也报告未初始化值的使用，最常见的消息是Conditional jump or move depends on uninitialised value(s)。可以难以确定这些错误的根源。尝试使用--track-origins=yes来获得额外的信息。这使得Memcheck运行得更慢，

Invalid write of size 1 //堆内存越界被查出来

Invalid free() / delete / delete[] //重复释放

Process terminating with default action of signal 11 (SIGSEGV): dumping core //非法指针，导致coredump

massif

堆栈分析器，指示程序中使用了多少堆内存等信息。它能测量程序在堆栈中使用了多少内存，告诉我们堆块，堆管理块和栈的大小。Massif能帮助我们减少内存的使用，在带有虚拟内存的现代系统中，它还能够加速我们程序的运行，减少程序停留在交换区中的几率。

Massif对内存的分配和释放做profile。程序开发者通过它可以深入了解程序的内存使用行为，从而对内存使用进行优化。这个功能对C++尤其有用，因为C++有很多隐藏的内存分配和释放

memcheck的常用选项

—tool= [default: memcheck]

最常用的选项。运行valgrind中名为toolname的工具。如果省略工具名，默认运行memcheck。

—leak-check= [default: summary]

用于控制内存泄漏检测力度。

• no，不检测内存泄漏；

• summary，仅报告总共泄漏的数量，不报告具体泄漏位置；

• yes/full，报告泄漏总数、泄漏的具体位置

—show-reachable= [default: no]

用于控制是否检测控制范围之外的泄漏，比如全局指针、static指针等。

—undef-value-errors= [default: yes]

用于控制是否检测代码中使用未初始化变量的情况。

—log-file=filename # 将结果输出到文件，支持相对路径

—track-fds= [default: no] # 跟踪打开的文件描述符

五种泄露内存的类型

• definitely lost：确认丢失。程序中存在内存泄露，应尽快修复。当程序结束时如果一块动态分配的内存没有被释放且通过程序内的指针变量均无法访问这块内存则会报这个错误。

• indirectly lost：间接丢失。当使用了含有指针成员的类或结构时可能会报这个错误。这类错误无需直接修复，他们总是与”definitely lost”一起出现，只要修复”definitely lost”即可。例子可参考我的例程。

• possibly lost：可能丢失。大多数情况下应视为与”definitely lost”一样需要尽快修复，除非你的程序让一个指针指向一块动态分配的内存（但不是这块内存起始地址），然后通过运算得到这块内存起始地址，再释放它。例子可参考我的例程。当程序结束时如果一块动态分配的内存没有被释放且通过程序内的指针变量均无法访问这块内存的起始地址，但可以访问其中的某一部分数据，则会报这个错误。

• still reachable：可以访问，未丢失但也未释放。如果程序是正常结束的，那么它可能不会造成程序崩溃，但长时间运行有可能耗尽系统资源，因此笔者建议修复它。如果程序是崩溃（如访问非法的地址而崩溃）而非正常结束的，则应当暂时忽略它，先修复导致程序崩溃的错误，然后重新检测。

• suppressed：已被解决。出现了内存泄露但系统自动处理了。可以无视这类错误。这类错误我没能用例程触发，看官方的解释也不太清楚是操作系统处理的还是valgrind，也没有遇到过。

操作MySQL结果集后释放内存

代码中获取MySQL结果常用由mysql_store_result，结果用valgrind检查时发现了内存泄露问题：

由mysql_store_result()、mysql_use_result()、mysql_list_dbs()获得结果集，在完成对结果集的操作后，必须调用mysql_free_result()释放结果集使用的内存。每次查询返回的结果的地址是不一样的，所以每次都要释放，否则会造成内存泄露。

1
2
3

mysql_free_result(result);
mysql_close(conn);
mysql_library_end();