【ROS教程 010】导航功能包集进阶

前面我们已经学习了创建功能包、节点和机器人的3D模型等。本节我们探索如何在自己的机器人上使用它们。真正的战斗才刚刚开始，疯狂打call吧！
注意：机器人创建功能包时需要添加必要的依赖项。
（1）创建机器人配置
在chapter8_tutorials/launch文件夹下创建chapter8_configuration_gazebo.launch，内容如下：

<?xml version="1.0"?>
<launch>

  <param name="/use_sim_time" value="true" />

<remap from="/base_scan/scan" to="/scan" />
  <!-- start up wg world -->
  <include file="$(find gazebo_worlds)/launch/wg_collada_world.launch"/>

  <arg name="model" default="$(find chapter8_tutorials)/urdf/robot1_base_04.xacro"/>
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
  <!-- start robot state publisher -->
  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen" />

<node name="spawn_robot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -z 0.1 -model robot_model" respawn="false" output="screen" />

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find chapter8_tutorials)/launch/navigation.vcg" />

</launch>

现在使用以下命令启动文件：

$ roslaunch chapter8_tutorials chapter8_configuration_gazebo.launch

（2）配置全局和局部代价地图

我们的机器人将使用两种导航算法在地图中移动——全局导航和局部导航。

全局导航：用于建立到地图上最终目标或一个远距离目标的路径。

局部导航：用于建立到近距离目标或者为了临时躲避障碍物的路径。

这些导航算法通过使用代价地图来处理地图中的各种信息。全局代价地图用于全局导航，局部代价地图用于局部导航。代价地图的参数用于配置算法计算行为。它们也有最基本的通用参数，这些参数保存在共享的文件中。我们可以在三个最基本的文件中设定不同的参数。这三个文件分别是

• costmap_common_params.yaml
• global_costmap_params.yaml
• local_costmap_params.yaml

step1：基本参数配置
在launch文件夹下创建costmap_common_params.yaml添加以下代码：

obstacle_range: 10
raytrace_range: 3.0
footprint: [[-0.2,-0.2],[-0.2,0.2], [0.2, 0.2], [0.2,-0.2]]
#robot_radius: ir_of_robot
inflation_radius: 0.55

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: laser_base_link, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

这个文件主要用于配置基本参数。这些参数被用于local_costmap和global_costmap。
obstacle_range和raytrace_range参数用于表示传感器的最大探测距离并在代价地图中引入探测的障碍信息。前者主要用于障碍的探测后者则用于在机器人运动过程中，实时清除代价地图中的障碍物，并更新可移动的自由空间数据。参数footprint用于将机器人的几何参数告知导航功能包集。这样就能够在机器人和障碍物之间保持一个合理的距离。inflation_radius
则给定机器人与障碍物之间必须保持的最小距离。我们还需要配置observation_sources
参数来设定导航功能包集所使用的传感器。只有配置了正确的传感器才能计算机器人在实际环境中的导航路径。
step2：全局代价地图的配置
在launch文件夹下创建一个global_costmap_params.yaml全局代价地图的配置，代码为：

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 5.0
  static_map: true

其中，global_frame和robot_base_frame参数用于定义机器人和地图之间的坐标变换。建立全局代价地图必须要使用这个变换。update_frequency配置代价地图更新的频率。
static_map用于配置是否使用一个地图或者地图服务器来初始化全局代价地图。
step3：局部代价地图的配置
在launch文件夹下的local_costmap_params.yaml文件用于配置局部代价地图。

local_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 5.0
  height: 5.0
  resolution: 0.1

参数 rolling_window 用于配置在机器人运动过程中，代价地图始终以机器人为中心。通过 width 、 height 和 resolution 参数来配置代价地图的尺寸和分辨率。
（3）基本局部规划器配置
完成了代价地图的配置能够开始一个基本规划器的配置了。这个基本规划器会产生一个速度命令来移动我们的机器人。在launch文件夹下创建base_local_planner_params.yaml代码如下：

TrajectoryPlannerROS:
  max_vel_x: 1
  min_vel_x: 0.8
  max_rotational_vel: 0.5
  min_in_place_rotational_vel: 0.2

  acc_lim_th: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5

  holonomic_robot: false

这个配置文件设定了我们机器人的最大和最小速度限值，同时也设定了加速度的限值。当使用一个全向移动机器人时，holonomic_robot设置为true。
（4）为导航功能包集创建启动文件
现在已经创建和配置了所有导航功能包集所需的文件。为运行导航功能包集我们还要创建一个启动文件move_base.launch。

<launch>

  <!-- Run the map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find chapter8_tutorials)/maps/map.yaml" output="screen"/>

  <include file="$(find amcl)/examples/amcl_diff.launch" >
  </include>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
  <rosparam file="$(find chapter8_tutorials)/launch/costmap_common_params.yaml" command="load" ns="global_costmap" />
  <rosparam file="$(find chapter8_tutorials)/launch/costmap_common_params.yaml" command="load" ns="local_costmap" />
  <rosparam file="$(find chapter8_tutorials)/launch/local_costmap_params.yaml" command="load" />
  <rosparam file="$(find chapter8_tutorials)/launch/global_costmap_params.yaml" command="load" />
  <rosparam file="$(find chapter8_tutorials)/launch/base_local_planner_params.yaml" command="load" />
  </node>

</launch>

注意，在这个文件中我们会启动之前创建的所有文件。现在启动文件，在两个不同的窗口中分别输入以下命令

s roslaunch chapter8_tutorials chapter8_configuration_gazebo.launch
s roslaunch chapter8_tutorials move_base.launch

如果你比较这时候的屏幕与之前你启动文件时所看到的屏幕就会发现所有的选项都变成蓝色的，这说明你的导航功能包集已经正常启动了。另外还会学习使用导航功能包集时通过窗口查看可视化数据所需的配置选项。
（5）为导航功能包集设置rviz

• num1：2D位姿估计

2D位姿估计允许用户初始化导航功能包集的定位系统，并设定机器人在实际环境中的位姿。导航功能包集会等待名为initialpose的新主题的初始化位姿消息。这个主题是通过rviz窗口发布的。点击2D Pose Estimate按钮，并点击地图来指定机器人的初始位姿。如果你不在一开始就进行这项工作，机器人会启动一个自动定位进程来初始位姿。
Topic: initialpose
Type: geometry_msgs/PoseWithCovarianceStamped

• num2:2D导航目标

2D导航目标允许用户向导航功能包集设定一个期望位姿作为机器人的导航目标。导航功能包会等待名为/move_base_simple/goal的新主题的初始化目标消息。因此，你必须在Tool Properties子窗口的2D Nav Goal选项中修改主题的名称。具体的配置方法为点击2D Nav Goal按钮并在地图上选择机器人的运动目标。你能够选择x轴和y轴坐标，以及机器人最后的方向。
Topic: move_base_simple/goal
Type: geometry_msgs/PoseStamped

导航功能包集在rviz中可以配置参数还包括：静态地图、点云、机器人立足点、障碍、膨胀障碍、全局规划、局部规划、规划器规划和当前目标。使用rxgraph命令查看是否所有节点都正确运行以及节点之间的相互关系。
（6）自适应蒙特卡罗定位
自适应蒙特卡罗定位是一种用于在2D环境下移动机器人的概率统计定位方法。这种算法在ROS系统中的具体实现是通过在已知地图的基础上使用粒子滤波算法跟踪机器人的位姿。ROS中的自适应蒙特卡罗定位算法在系统中有很多个配置选项。这些选项对定位算法的性能影响较大。
amcl_diff.launch文件对其参数进行一系列的配置。具体的配置信息有时间再研究。

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

（7）发送目标
在某些场景我们需要通过编程设定一系列的运动指令，让机器人去特定地方。actionlib功能包提供了标准的接口。在src下创建新文件sendGoals.cpp。

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_broadcaster.h>
#include <sstream>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv){
ros::init(argc, argv, "navigation_goals");

MoveBaseClient ac("move_base", true);

while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server");
}

move_base_msgs::MoveBaseGoal goal;

goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();

goal.target_pose.pose.position.x = 1.0;
goal.target_pose.pose.position.y = 1.0;
goal.target_pose.pose.orientation.w = 1.0;

ROS_INFO("Sending goal");
ac.sendGoal(goal);

ac.waitForResult();

if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("You have arrived to the goal position");
else{
ROS_INFO("The base failed for some reason");
}
return 0;
}

使用启动文件启动仿真环境并测试新程序。

$ roslaunch chapter8_tutorials chapter8_configuration_gazebo.launch
$ roslaunch chapter8_tutorials move_base.launch
$ rosrun chapter8_tutorials sendGoals

参考文献:
[1]. Aaron Martinez Enrique Fern andez, ROS机器人程序设计[B], P184-202, 2014.
http://wiki.ros.org/amcl
http://probabilistic-robotics.org/