2.7.机器人SLAM-建图与导航

发布里程计的TF变换

在trd_driver驱动的ROS主循环中，加入发布base_link–>odom之间的TF变换的代码。

# 发布里程的tf变换
self.tf_broadcaster.sendTransform(
    (self.x,self.y,0),
    tf.transformations.quaternion_from_euler(0, 0, self.theta),
    rospy.Time.now(),
    'base_link',
    'odom')

利用激光进行建图

建图框架如下图所示：

现在，我们已经实现了电机驱动节点、激光驱动节点的编写和安装。接下来需要加入建图节点实现完整的建图功能。

• 电机驱动节点(trd_driver.py)

针对不同的驱动，需要启动不同的电机驱动。 在此，我们采用前几课开发的针对坦克底盘的驱动，trd_driver包下的trd_driver.py节点。

• 激光驱动节点(ls01d)

在此，激光传感器采用深圳镭神开发的ls01d型号，请参照之前课程驱动包的安装及启动方法。

• 建图节点(gmapping或hector_mapping)

建图节点读取电机驱动节点发布的/odom里程计消息，以及激光驱动节点发布的/scan激光数据消息，即可完成建图，输出/map地图消息。 ROS中常用的移动底盘建图算法有gmapping和hector_mapping，两者的ROS地址为：

http://wiki.ros.org/slam_gmapping

http://wiki.ros.org/hector_mapping

安装方式即通过apt install指令安装：

$ sudo apt install ros-kinetic-slam-gmapping

$ sudo apt install ros-kinetic-hector-mapping

可通过rosrun指令启动：

$ rosrun gmapping slam_gmapping

$ rosrun hector_mapping hector_mapping

当然，最好的方式是在launch文件中统一启动。

手动遥控方式建图

在使用底盘进行建图时，可通过键盘遥控的方式，控制底盘扫描周围环境，实现建图。 此时，只需启动电机驱动节点、激光驱动节点以及建图节点即可，将3个节点在一个launch文件中启动， launch文件内容如下：

<?xml version="1.0"?>
<launch>
    <param name="use_sim_time" value="false" />
    <include file="$(find trd_driver)/launch/trd_control.launch">
        <arg name="serialport" value="/dev/motor_trd" />
    </include>
    <include file="$(find ls01d)/launch/ls01d.launch" />
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" />
</launch>

在树莓派启动mapping.launch文件，然后在笔记本电脑启动RViz进行查看。

$ roslaunch trd_driver mapping.launch

$ rviz

地图的保存

在建图过程中，随时可以执行map_server包下的map_saver节点完成地图的保存。

$ rosrun map_server map_saver

默认会保存为map.yaml和map.pgm文件。

打开map.yaml文件，查看文件内容，了解其参数的意义。

运动规划节点

运动规划完成的功能为：接收到目标点后，结合当前地图信息，以及激光等传感器检测到的障碍物信息， 规划出如何到达目标点的路径（称为全局路径），同时避开沿途障碍物。并能根据全局路径， 计算出每一时刻底盘应该行进的角度及方向（称为局部路径）。

在ROS的navigation地面导航包中，其核心是运动规划节点，即move_base。

安装navigation超包(即一系列相关功能包组成的包)后，会安装move_base包及节点。

$ sudo apt install ros-kinetic-navigation

由于move_base节点启动时，需要加载很多算法参数，所以通常会在launch文件中启动， 并指定其yaml参数配置文件。

建图后，在已有地图上进行导航

与建图功能的launch文件相比，导航功能不需要建图节点了，但需要具备以下节点：

• 电机驱动节点（trd_driver.py）
• 激光驱动节点（ls01d）
• 地图文件发布节点（map_server）
• 机器人定位节点（amcl）
• 运动控制节点（move_base）

在trd_driver包的launch文件夹下，新建navigation.launch文件，并将各节点调用添加到launch文件中：

<?xml version="1.0"?>
<launch>
    <param name="use_sim_time" value="false" />
    <include file="$(find trd_driver)/launch/trd_control.launch">
        <arg name="serialport" value="/dev/motor_trd" />
    </include>
    <include file="$(find ls01d)/launch/ls01d.launch" />
    <node pkg="map_server" type="map_server" name="map_server"
          args="$(find trd_driver)/maps/map.yaml" />
    <node pkg="amcl" type="amcl" name="amcl" />
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find trd_driver)/config/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find trd_driver)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find trd_driver)/config/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

launch文件以来4个配置文件，里面设定了算法配置等参数， 在trd_driver包下新建config文件夹，并在config文件夹下加入以下4个文件：

• costmap_common_params.yaml

bstacle_range: 2.5
raytrace_range: 3.0
#robot_radius: 0.32
footprint: [[0.3,0.25], [0.3,0], [0.3,-0.25],[-0.3,-0.25],[-0.3,0.25]]

max_obstacle_height: 0.6
min_obstacle_height: 0.0

obstacle_layer:
  observation_sources: laser_scan_sensor
  laser_scan_sensor:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    expected_update_rate: 0

• local_costmap_params.yaml

local_costmap:
   inflation_radius: 0.25
   global_frame: /odom
   robot_base_frame: /base_link
   update_frequency: 2.0
   publish_frequency: 1.0
   static_map: false
   rolling_window: true
   width: 6.0
   height: 6.0
   resolution: 0.025
   transform_tolerance: 1.0
   map_type: costmap
   plugins:
     - {name: obstacle_layer,     type: "costmap_2d::ObstacleLayer"}
#    - {name: range_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"}
#    - {name: inflation_layer,    type: "costmap_2d::InflationLayer"}

• global_costmap_params.yaml

global_costmap:
   inflation_radius: 0.25
   global_frame: /map
   robot_base_frame: /base_link
   update_frequency: 1.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   resolution: 0.025
   transform_tolerance: 1.0
   map_type: costmap
   plugins:
     - {name: static_layer,     type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,     type: "costmap_2d::ObstacleLayer"}
     - {name: inflation_layer,    type: "costmap_2d::InflationLayer"}

• base_local_planner_params.yaml

planner_frequency: 0.3
controller_frequency: 2.0
recovery_behavior_enabled: true
clearing_rotation_allowed: true
controller_patience: 60

recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'

conservative_reset:
  reset_distance: 1.0

aggressive_reset:
  reset_distance: 0.0

TrajectoryPlannerROS:
   max_vel_x: 0.35
   min_vel_x: 0.0
   max_vel_y: 0.0  # zero for a differential drive robot
   min_vel_y: 0.0
   max_vel_theta: 0.5
   min_vel_theta: -0.5
   min_in_place_vel_theta: 0.3
   escape_vel: -0.1
   acc_lim_x: 1.0
   acc_lim_y: 0.0 # zero for a differential drive robot
   acc_lim_theta: 3.2

   holonomic_robot: false
   yaw_goal_tolerance: 0.2 # about 6 degrees/0.1
   xy_goal_tolerance: 0.3  #
   latch_xy_goal_tolerance: false
   pdist_scale: 0.8
   gdist_scale: 0.4
   meter_scoring: true

   heading_lookahead: 1.0
   heading_scoring: false
   heading_scoring_timestep: 0.8
   occdist_scale: 0
   oscillation_reset_dist: 0.05
   publish_cost_grid_pc: false
   prune_plan: true

   sim_time: 3.0
   sim_granularity: 0.025
   angular_sim_granularity: 0.025
   vx_samples: 16
   vy_samples: 0 # zero for a differential drive robot
   vtheta_samples: 30
   dwa: true
   simple_attractor: false

在树莓派启动navigation.launch文件，然后在笔记本电脑启动RViz进行查看。

$ roslaunch trd_driver navigation.launch

$ rviz

点击RViz界面的Nav Goal按钮，并在地图上设置目标位置，底盘即可自动导航至指定位置。

move_base原理

• 全局规划算法

功能：已知起始点、目标点、地图，计算出起始点到目标点的路径。

输出：一条路径（/plan），消息类型nav_msgs/Path

• 局部规划算法

功能：已知全局规划路线，计算底盘的实时运动速度，保证能够按照路线行走。

输出：运动速度（/cmd_vel），消息类型geometry_msgs/Twist

另一种建图方式：自动探索式建图

只需要在mapping.launch文件中加入move_base节点即可。

<?xml version="1.0"?>
<launch>
    <param name="use_sim_time" value="false" />
    <include file="$(find trd_driver)/launch/trd_control.launch">
        <arg name="serialport" value="/dev/motor_trd" />
    </include>
    <include file="$(find ls01d)/launch/ls01d.launch" />
    <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" />
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find trd_driver)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find trd_driver)/config/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find trd_driver)/config/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find trd_driver)/config/base_local_planner_params.yaml" command="load" />
    </node>
</launch>

点击RViz界面的Nav Goal按钮，并在地图上设置目标位置，底盘即可一边建图，一边自动导航至指定位置。