# 10.5 配置Fishbot进行建图 上一节我们安装好了cartographer，这节课我们就开始配置cartographer进行建图。 我们需要创建一个功能包，将参数文件和`Cartographer`启动文件放到一起然后启动。 ![cartographer](10.5%E9%85%8D%E7%BD%AEFishbot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE/imgs/cartographer.gif) ## 1.创建fishbot_cartographer 在src目录下，使用创建功能包指令，创建功能包 ```shell cd src ros2 pkg create fishbot_cartographer ``` 接着创建配置文件夹、launch文件夹和rviz配置文件夹。 ```shell cd fishbot_cartographer mkdir config mkdir launch mkdir rviz ``` 创建完成的功能包结构 ``` . ├── CMakeLists.txt ├── config ├── launch ├── src ├── package.xml └── rviz ``` ## 2.添加cartographer配置文件 在`fishbot/config`目录下创建`fishbot_2d.lua`文件。 接着我们来写一下配置文件，相较于默认的配置文件，主要修改以下内容（见注释） ```lua include "map_builder.lua" include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link", -- base_link改为odom,发布map到odom之间的位姿态 published_frame = "odom", odom_frame = "odom", -- true改为false，不用提供里程计数据 provide_odom_frame = false, -- false改为true，仅发布2D位资 publish_frame_projected_to_2d = true, -- false改为true，使用里程计数据 use_odometry = true, use_nav_sat = false, use_landmarks = false, -- 0改为1,使用一个雷达 num_laser_scans = 1, -- 1改为0，不使用多波雷达 num_multi_echo_laser_scans = 0, -- 10改为1，1/1=1等于不分割 num_subdivisions_per_laser_scan = 1, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., landmarks_sampling_ratio = 1., } -- false改为true，启动2D SLAM MAP_BUILDER.use_trajectory_builder_2d = true -- 0改成0.10,比机器人半径小的都忽略 TRAJECTORY_BUILDER_2D.min_range = 0.10 -- 30改成3.5,限制在雷达最大扫描范围内，越小一般越精确些 TRAJECTORY_BUILDER_2D.max_range = 3.5 -- 5改成3,传感器数据超出有效范围最大值 TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. -- true改成false,不使用IMU数据，大家可以开启，然后对比下效果 TRAJECTORY_BUILDER_2D.use_imu_data = false -- false改成true,使用实时回环检测来进行前端的扫描匹配 TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true -- 1.0改成0.1,提高对运动的敏感度 TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) -- 0.55改成0.65,Fast csm的最低分数，高于此分数才进行优化。 POSE_GRAPH.constraint_builder.min_score = 0.65 --0.6改成0.7,全局定位最小分数，低于此分数则认为目前全局定位不准确 POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 -- 设置0可关闭全局SLAM -- POSE_GRAPH.optimize_every_n_nodes = 0 return options ``` ## 3.添加launch文件 ### 3.1 launch需要包含的节点 要完成使用Cartographer进行建图，需要两个节点的参与，整个过程的计算流图如下： ![image-20220503212253994](10.5%E9%85%8D%E7%BD%AEFishbot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE/imgs/image-20220503212253994.png) **/cartographer_node节点:** 该节点从/scan和/odom话题接收数据进行计算，输出/submap_list数据. 该节点需要接收一个参数配置文件（第二部分写的那个）参数。 **/occupancy_grid_node节点：** 该节点接收/submap_list子图列表，然后将其拼接成map并发布 该节点需要配置地图分辨率和更新周期两个参数。 ### 3.2 编写launch文件 在路径`src/fishbot_cartographer/launch/`下新建`cartographer.launch.py`文件，接着我们将上面两个节点加入到这个launch文件中。 我们在第二部分写的配置文件就是给cartographer_node节点的，可以通过这个节点启动参数`configuration_directory`和`configuration_basename`进行传递。 ``` import os from launch import LaunchDescription from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # 定位到功能包的地址 pkg_share = FindPackageShare(package='fishbot_cartographer').find('fishbot_cartographer') #=====================运行节点需要的配置======================================================================= # 是否使用仿真时间，我们用gazebo，这里设置成true use_sim_time = LaunchConfiguration('use_sim_time', default='true') # 地图的分辨率 resolution = LaunchConfiguration('resolution', default='0.05') # 地图的发布周期 publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') # 配置文件夹路径 configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') ) # 配置文件 configuration_basename = LaunchConfiguration('configuration_basename', default='fishbot_2d.lua') #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node================================= cartographer_node = Node( package='cartographer_ros', executable='cartographer_node', name='cartographer_node', output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=['-configuration_directory', configuration_directory, '-configuration_basename', configuration_basename]) occupancy_grid_node = Node( package='cartographer_ros', executable='occupancy_grid_node', name='occupancy_grid_node', output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]) rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', # arguments=['-d', rviz_config_dir], parameters=[{'use_sim_time': use_sim_time}], output='screen') #===============================================定义启动文件======================================================== ld = LaunchDescription() ld.add_action(cartographer_node) ld.add_action(occupancy_grid_node) ld.add_action(rviz_node) return ld ``` ## 4.添加安装指令 做完上面的操作，我们还需要添加安装指令。 打开CmakeLists.txt，添加下面一条指令，将三个目录安装到install目录。 ``` install( DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME} ) ``` ## 5.开始建图 ### 5.1编译启动 ```shell colcon build --packages-select fishbot_cartographer ``` 启动建图前，需要先启动gazebo仿真环境，因为我们的建图程序依赖于Gazebo提供雷达和里程计等数据。 ```shell source install/setup.bash ros2 launch fishbot_description gazebo.launch.py ``` source,启动建图 ```shell source install/setup.bash ros2 launch fishbot_cartographer cartographer.launch.py ``` ### 5.2修改配置 如果一切正常，你应该看到的是一个空空如也的RVIZ界面 ![image-20220503221911478](10.5%E9%85%8D%E7%BD%AEFishbot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE/imgs/image-20220503221911478.png) 不用担心，此时地图其实已经有了，我们需要添加一下地图相关的插件即可。 通过`Add`->`By Topic`添加组件。 ![image-20220503222230429](10.5%E9%85%8D%E7%BD%AEFishbot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE/imgs/image-20220503222230429.png) 最后通过左边的插件你应该可以看到图和机器人了。 ![image-20220503222158266](10.5%E9%85%8D%E7%BD%AEFishbot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE/imgs/image-20220503222158266.png) ### 5.3开始建图 打开我们机器人遥控节点，降低速度，控制机器人走一圈，看看地图的变化。 ``` ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` ![cartographer](10.5%E9%85%8D%E7%BD%AEFishbot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE/imgs/cartographer.gif) ### 6.保存地图 走完一圈，没有黑影部分，我们就可以保存地图为一个本地文件了。我们需要使用一个叫做`nav2_map_server`的工具。 ### 6.1 安装nav2_map_server ``` sudo apt install ros-foxy-nav2-map-server ``` ### 6.2 保存地图 ``` ros2 run nav2_map_server map_saver_cli --help ``` 可以看到有下面的用法 ```shell Usage: map_saver_cli [arguments] [--ros-args ROS remapping args] Arguments: -h/--help -t -f --occ --free --fmt --mode trinary(default)/scale/raw NOTE: --ros-args should be passed at the end of command line ``` 我们的地图话题为map，文件名字我们用fishbot_map,所以有下面这个这样写的命令行。 ```shell # 先将地图保存到src/fishbot_cartographer/map目录下 cd src/fishbot_cartographer/ && mkdir map && cd map ros2 run nav2_map_server map_saver_cli -t map -f fishbot_map ``` 接着我们就可以得到下面的两个文件 ```shell . ├── fishbot_map.pgm └── fishbot_map.yaml 0 directories, 2 files ``` 这两个文件就是对当前地图保存下来的文件，下面的导航我们将要使用到这两个文件。 ## 7.总结 本节小鱼带你一起完成了Fishbot的Cartographer配置和建图，下一节小鱼会对地图是什么以及如何使用进行介绍，接着我们就可以配置Nav2进行Fishbot的导航了。 -------------- 技术交流&&问题求助： - **微信公众号及交流群：鱼香ROS** - **小鱼微信：AiIotRobot** - **QQ交流群：139707339** - 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划