ROS高效进阶第六章 -- 机器人SLAM建图与自主导航之move_base

1 背景资料

本文是机器人SLAM建图与自主导航的第三篇文章，我们将基于之前ROS高效进阶第六章 – 机器人SLAM建图与自主导航之gmapping算法的 mbot_navigation 样例进行扩充，使用 ros 的 move_base 导航与路径规划包，在有地图和没地图两种情况下，控制轮式机器人探索整个屋子。本文的测试环境是ubuntu20.04 + ros noetic。
本文参考资料如下：
（1）《ROS机器人开发实践》胡春旭 第9章
（2）ROS高效进阶第六章 – 机器人SLAM建图与自主导航之gmapping算法
（3）ROS高效进阶第六章 – Ros Action通信机制
（4）Turtlebot的建图导航之旅（宝藏文章）
（5）ROS move_base
（6）ROS Amcl
（7）ROS Navigation
（8）amcl的原理，可参考《概率机器人》

2 正文

2.1 move_base导航规划框架

（1）下面的图是move_base导航规划框架，中间白色框内是move_base，左右两边是它的依赖，蓝色图框是必须依赖，灰色图框是可选依赖。下面是它的输出，也就是经典的/cmd_vel topic，发送给机器人控制器。

安装 ros navigation包：

sudo apt-get install ros-noetic-navigation

（2）move_base依赖 lidar 识别周边环境，依赖里程计确定自己的方位。但 move_base 并不强依赖map_server的全局地图，没有地图也可以边建图边导航。如果move_base 使用了全局地图，就需要使用amcl（Adaptive Monte Carlo Localization，自适应蒙特卡罗定位）进行自身地位，确定自身在全局地图中的位置。
（3）一个机器人从起点到终点需要解决两个问题，一是路由/路线导航，二是轨迹规划。前者对应move_base的global planner，使用经典的Dijkstra或A*算法；后者对应move_base的local_planner，使用Trajectory Rollout 和 Dynamic Window Approaches。
除此之外，recovery_vehaviors是move_base的脱困功能，在机器人卡死后，进行自主脱困。
（4）在自动驾驶领域，路线导航由导航模块依赖车载地图完成。轨迹规划依赖planning模块，这是自动驾驶领域的关键，其使用路径-速度解耦规划算法，Lattice规划算法等。
（5）如果想系统的学习move_base导航规划框架，这里推荐一个宝藏博客：Turtlebot的建图导航之旅（宝藏文章）。如果想更深入学习，请去ros网站学习，链接在第一章。

2.2 mbot_navigation 样例

（1）创建相关launch文件，和控制程序文件（cpp，py各一份）

cd ~/catkin_ws/src/mbot_navigation
mkdir scritps
touch launch/amcl.launch launch/move_base.launch 
touch launch/navigation_with_map.launch launch/navigation_with_gmapping.launch
touch src/exploring_house.cpp scripts/exploring_house.py

（2）amcl.launch（所有参数一律来自官方样例）

<launch>
    <arg name="scan_topic" default="scan"/>
    <arg name="initial_pose_x" default="0.0"/>
    <arg name="initial_pose_y" default="0.0"/>
    <arg name="initial_pose_a" default="0.0"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="min_particles"             value="500"/>
        <param name="max_particles"             value="3000"/>
        <param name="kld_err"                   value="0.02"/>
        <param name="update_min_d"              value="0.20"/>
        <param name="update_min_a"              value="0.20"/>
        <param name="resample_interval"         value="1"/>
        <param name="transform_tolerance"       value="0.5"/>
        <param name="recovery_alpha_slow"       value="0.00"/>
        <param name="recovery_alpha_fast"       value="0.00"/>
        <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
        <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
        <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
        <param name="gui_publish_rate"          value="50.0"/>

        <remap from="scan"                      to="$(arg scan_topic)"/>
        <param name="laser_max_range"           value="3.5"/>
        <param name="laser_max_beams"           value="180"/>
        <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_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type"          value="likelihood_field"/>

        <param name="odom_model_type"           value="diff"/>
        <param name="odom_alpha1"               value="0.1"/>
        <param name="odom_alpha2"               value="0.1"/>
        <param name="odom_alpha3"               value="0.1"/>
        <param name="odom_alpha4"               value="0.1"/>
        <param name="odom_frame_id"             value="odom"/>
        <param name="base_frame_id"             value="base_footprint"/>
    node>
launch>

（2）move_base.launch如下，里面的配置参数文件来自官方样例，见：mbot_navigation

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />

    <rosparam file="$(find mbot_navigation)/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mbot_navigation)/config/move_base/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/move_base/global_costmap_params.yaml" command="load" />

    <rosparam file="$(find mbot_navigation)/config/move_base/move_base_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/move_base/dwa_local_planner_params.yaml" command="load" />
  node>
 
launch>

（3）navigation_with_map.launch实现了在有全局地图下的导航和规划

<launch>
    
    <arg name="map" default="gmapping_save.yaml" />

    
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

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

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

launch>

（4）navigation_with_gmapping.launch实现了无全局地图下的导航和规划

<launch>
    <include file="$(find mbot_navigation)/launch/gmapping.launch"/>

    
    <include file="$(find mbot_navigation)/launch/move_base.launch" />

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

（5）exploring_house.cpp作为move_base action服务的客户端，用来向move_base发送目标点。ros action的原理和使用方法见：ROS高效进阶第六章 – Ros Action通信机制

#include 
#include 
#include 
#include 
#include 

geometry_msgs::Pose createPose(double px, double py, double pz, double ox, double oy, double oz, double ow) {
    geometry_msgs::Pose pose;
    pose.position.x = px;
    pose.position.y = py;
    pose.position.z = pz;
    pose.orientation.x = ox;
    pose.orientation.y = oy;
    pose.orientation.z = oz;
    pose.orientation.w = ow;
    return pose;  
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "move_test");
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client("move_base", true);

  ROS_INFO("Waiting for move_base action server...");  
  move_base_client.waitForServer();
  ROS_INFO("connected to move base server");

  std::vector<geometry_msgs::Pose> target_list;
  target_list.push_back(createPose(6.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(-5.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(-5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));

  for (uint8_t i = 0; i < target_list.size(); i ++) {
    ros::Time start_time = ros::Time::now();
    ROS_INFO("going to %u goal, position: (%f, %f)", i, target_list[i].position.x, target_list[i].position.y);

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose = target_list[i];

    move_base_client.sendGoal(goal);
    move_base_client.waitForResult();

    if (move_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
      ros::Duration running_time =  ros::Time::now() - start_time;
      ROS_INFO("go to %u goal succeeded, running time %f sec", i, running_time.toSec());
    } else {
      ROS_INFO("goal failed");
    }
  }

  return 0;
}

（6）exploring_house.py的功能同exploring_house.cpp，只是用python实现一遍

#!/usr/bin/env python3
import rospy
import actionlib
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  

def main():
  rospy.init_node("move_test", anonymous=True)
  move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  rospy.loginfo("Waiting for move_base action server...")  
  while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("connected to move base server")
  
  target_list = []
  target_list.append(Pose(Point(6.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  target_list.append(Pose(Point(5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  target_list.append(Pose(Point(-5.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  target_list.append(Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  
  for i, target in enumerate(target_list):
    start_time = rospy.Time.now()  
    
    goal = MoveBaseGoal()
    goal.target_pose.pose = target
    goal.target_pose.header.frame_id = 'map'
    goal.target_pose.header.stamp = rospy.Time.now()
    
    rospy.loginfo("going to {0} goal, {1}".format(i, str(target)))
    move_base_client.send_goal(goal)
    finished_within_time = move_base_client.wait_for_result(rospy.Duration(300))
    
    if not finished_within_time:
      move_base_client.cancel_goal()
      rospy.loginfo("time out, failed to goal")
    else:
      running_time = (rospy.Time.now() - start_time).to_sec()
      if move_base_client.get_state() == GoalStatus.SUCCEEDED:
        rospy.loginfo("go to {0} goal succeeded, run time: {1} sec".format(i, running_time))
      else:
        rospy.loginfo("goal failed")
    
if __name__ == "__main__":
  main()

（7）CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(mbot_navigation)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  move_base_msgs
  roscpp
  rospy
  actionlib
)
catkin_package()
include_directories(
  ${catkin_INCLUDE_DIRS}
)
add_executable(exploring_house src/exploring_house.cpp)
target_link_libraries(exploring_house
  ${catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
  scripts/exploring_house.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

（8）编译和运行

cd ~/catkin_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="mbot_navigation;mbot_gazebo"
source devel/setup.bash

// 打开仿真环境
roslaunch mbot_gazebo mbot_gazebo.launch
//再开一个窗口，运行有全局地图下的导航规划样例
roslaunch mbot_navigation navigation_with_map.launch
// 再开一个窗口，控制机器人行动
rosrun mbot_navigation exploring_house

// 打开仿真环境
roslaunch mbot_gazebo mbot_gazebo.launch
//再开一个窗口，运行有无全局地图下的导航规划样例
roslaunch mbot_navigation navigation_with_gmapping.launch
// 再开一个窗口，控制机器人行动
rosrun mbot_navigation exploring_house.py

3 总结

本文代码托管在本人的github上：mbot_navigation