ROS开发笔记（7）——利用amcl、move_base 进行导航、基于Python编写巡逻机器人导航代码

版权声明：转载请注明出处，谢谢。 https://blog.csdn.net/wsc820508/article/details/81627858

在前期构建地图的基础上，本文将利用 amcl（自适应蒙特卡洛定位算法）来在地图中定位机器人，然后在此基础上利用move_base 在地图中对机器人导航。主要内容如下：

1、amcl（自适应蒙特卡洛定位算法）节点基本原理

2、move_base 节点基本工作原理

3、在地图中定位机器人

4、在rivz中导航

5、编写代码导航

1、amcl（自适应蒙特卡洛定位算法）节点基本原理

1.1、粒子云与pose估计值

amcl算法是一种粒子滤波算法，其维护一堆的位姿（pose）值用粒子（下图中机器人周围的绿色点状物的便是粒子云）表示，每个粒子具有一个概率值，概率越大代表机器人处于这个pose的可能性越大，按概率加权平均就得到了机器人pose的估计值。

1.2、运动预测更新

随着机器人的移动，各个粒子也跟随着机器人里程计数据一起运动更新。

1.3、量测更新

实际量测值：实际的机器人的传感器数据（如激光扫描scan数据）。

理想量测值：基于地图模型map数据可以算出如果机器人就处于某个粒子pose时，理想的传感器数据应该是什么样的。

如果某个粒子的|理想量测值-实际量测值| 比较小，那么这个粒子关联的概率值就要大，反之小，这样对粒子云的概率值进行更新，随着时间的推进，概率过小的粒子将被剔除，概率大的保留并重采样，这样粒子云的分布就发生了变化，如果定位算法收敛，其分布会逐步集中，中心收敛到真实pose附近。

amcl 节点信息如下：

wsc@wsc-pc:~$ rosnode list
/amcl
/gazebo
/gazebo_gui
/map_server
/move_base
/robot_state_publisher
/rosout
/rviz
/turtlebot3_teleop_keyboard
wsc@wsc-pc:~$ rosnode info amcl
--------------------------------------------------------------------------------
Node [/amcl]
Publications: 
 * /amcl/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /amcl/parameter_updates [dynamic_reconfigure/Config]
 * /amcl_pose [geometry_msgs/PoseWithCovarianceStamped]
 * /particlecloud [geometry_msgs/PoseArray]
 * /rosout [rosgraph_msgs/Log]
 * /tf [tf2_msgs/TFMessage]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]
 * /initialpose [geometry_msgs/PoseWithCovarianceStamped]
 * /scan [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

Services: 
 * /amcl/get_loggers
 * /amcl/set_logger_level
 * /amcl/set_parameters
 * /global_localization
 * /request_nomotion_update
 * /set_map


contacting node http://wsc-pc:46609/ ...
Pid: 12958
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /tf
    * to: /amcl
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /move_base
    * direction: outbound
    * transport: TCPROS
 * topic: /tf
    * to: /rviz
    * direction: outbound
    * transport: TCPROS
 * topic: /particlecloud
    * to: /rviz
    * direction: outbound
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf
    * to: /amcl (http://wsc-pc:46609/)
    * direction: inbound
    * transport: INTRAPROCESS
 * topic: /tf
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf
    * to: /robot_state_publisher (http://wsc-pc:41277/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf_static
    * to: /robot_state_publisher (http://wsc-pc:41277/)
    * direction: inbound
    * transport: TCPROS
 * topic: /scan
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /initialpose
    * to: /rviz (http://wsc-pc:45675/)
    * direction: inbound
    * transport: TCPROS

wsc@wsc-pc:~$ 

2、move_base 节点基本工作原理

参考链接：http://wiki.ros.org/move_base?distro=melodic

move_base综合了地图、定位系统、传感器及里程计数据，规划一条从当前位置到目标位置goal的路径。

1、navigation goal

通过 MoveBaseAction , MoveBaseGoal等实现，MoveBaseGoal封装了目标pose数据，客服端程序将MoveBaseGoal类型数据发送给MoveBaseAction服务端启动导航过程。

2、global planner

全局规划器 利用地图与规划算法规划一条从当前位置到目标位置的最优路径。

3、local planner

可以利用传感器数据识别不在地图中障碍物信息，如果本地规划行不通，会请求重新进行全局规划。

move_base与其他组件的相关接口描述如下：

rosnode info move_base
--------------------------------------------------------------------------------
Node [/move_base]
Publications: 
 * /cmd_vel [geometry_msgs/Twist]
 * /move_base/DWAPlannerROS/cost_cloud [sensor_msgs/PointCloud2]
 * /move_base/DWAPlannerROS/global_plan [nav_msgs/Path]
 * /move_base/DWAPlannerROS/local_plan [nav_msgs/Path]
 * /move_base/DWAPlannerROS/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/DWAPlannerROS/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/DWAPlannerROS/trajectory_cloud [sensor_msgs/PointCloud2]
 * /move_base/NavfnROS/plan [nav_msgs/Path]
 * /move_base/current_goal [geometry_msgs/PoseStamped]
 * /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
 * /move_base/global_costmap/costmap [nav_msgs/OccupancyGrid]
 * /move_base/global_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]
 * /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base/global_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/global_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/global_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/global_costmap/static_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/static_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/goal [move_base_msgs/MoveBaseActionGoal]
 * /move_base/local_costmap/costmap [nav_msgs/OccupancyGrid]
 * /move_base/local_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]
 * /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base/local_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/local_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/local_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/local_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/local_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/local_costmap/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/result [move_base_msgs/MoveBaseActionResult]
 * /move_base/status [actionlib_msgs/GoalStatusArray]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]
 * /map [nav_msgs/OccupancyGrid]
 * /move_base/cancel [unknown type]
 * /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base/goal [move_base_msgs/MoveBaseActionGoal]
 * /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base_simple/goal [geometry_msgs/PoseStamped]
 * /odom [nav_msgs/Odometry]
 * /scan [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

Services: 
 * /move_base/DWAPlannerROS/set_parameters
 * /move_base/NavfnROS/make_plan
 * /move_base/clear_costmaps
 * /move_base/get_loggers
 * /move_base/global_costmap/inflation_layer/set_parameters
 * /move_base/global_costmap/obstacle_layer/set_parameters
 * /move_base/global_costmap/set_parameters
 * /move_base/global_costmap/static_layer/set_parameters
 * /move_base/local_costmap/inflation_layer/set_parameters
 * /move_base/local_costmap/obstacle_layer/set_parameters
 * /move_base/local_costmap/set_parameters
 * /move_base/make_plan
 * /move_base/set_logger_level
 * /move_base/set_parameters


contacting node http://wsc-pc:41569/ ...
Pid: 12968
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /cmd_vel
    * to: /gazebo
    * direction: outbound
    * transport: TCPROS
 * topic: /move_base/goal
    * to: /move_base
    * direction: outbound
    * transport: INTRAPROCESS
 * topic: /move_base/NavfnROS/plan
    * to: /rviz
    * direction: outbound
    * transport: TCPROS
 * topic: /move_base/local_costmap/footprint
    * to: /rviz
    * direction: outbound
    * transport: TCPROS
 * topic: /move_base/DWAPlannerROS/global_plan
    * to: /rviz
    * direction: outbound
    * transport: TCPROS
 * topic: /move_base/DWAPlannerROS/local_plan
    * to: /rviz
    * direction: outbound
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf
    * to: /robot_state_publisher (http://wsc-pc:41277/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf
    * to: /amcl (http://wsc-pc:46609/)
    * direction: inbound
    * transport: TCPROS
 * topic: /tf_static
    * to: /robot_state_publisher (http://wsc-pc:41277/)
    * direction: inbound
    * transport: TCPROS
 * topic: /move_base_simple/goal
    * to: /rviz (http://wsc-pc:45675/)
    * direction: inbound
    * transport: TCPROS
 * topic: /map
    * to: /map_server (http://wsc-pc:33113/)
    * direction: inbound
    * transport: TCPROS
 * topic: /scan
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /odom
    * to: /gazebo (http://wsc-pc:33887/)
    * direction: inbound
    * transport: TCPROS
 * topic: /move_base/goal
    * to: /move_base (http://wsc-pc:41569/)
    * direction: inbound
    * transport: INTRAPROCESS

3、在地图中定位机器人

这一步需要启动以下文件：

需要启动的文件

roslaunch turtlebot3_gazebo turtlebot3_world.launch

roslaunch turtlebot3_navigation turtlebot3_navigation.launch

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

其中 turtlebot3_navigation turtlebot3_navigation.launch 启动文件中启动了map_server 、amcl 、move_base等节点，注意根据自己情况配置好map_file参数（map文件位置），否则会报找不到地图文件。另外，启动move_base节点时一开始报了一个错误“costmap params Invalid argument "/map" passed to canTransform”，根据错误提示，解决方法是，参数配置文件 global_costmap_params.yaml 与 global_costmap_params.yaml 中类似于 “global_frame:  /map ” 改为    “global_frame:  map ”，两个文件中各需要修改两处地方。

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/>
  <arg name="open_rviz" default="true"/>

  <!-- Turtlebot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>

  <!-- AMCL -->
  <include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>

  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
  </group>
</launch>

为了使算法收敛，初始位置误差不要太大，初始误差大小可以看激光传感器数据与地图障碍物信息的位置误差，启动roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch手动驱动机器人行走，可以使得观察数据多样化加速amcl的收敛。具体操作过程看下面的动图。

4、在rivz中导航

以下是在rivz中导航操作过程的动图：

整个系统节点交连图如下：

5、编写代码导航

通过编程实现两个目标点间来回巡逻的机器人导航代码及注释如下：

#!/usr/bin/env python
#-*- coding:utf-8   -*-


import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import tf

# 巡逻点
waypoints=[
    [(1.6,0.5,0.0),(0.0,0.0,100.0)],
    [(-1.8,-0.6,0.0),(0.0,0.0,180.0)]
]
def goal_pose(pose):
    goal_pose=MoveBaseGoal()
    goal_pose.target_pose.header.frame_id="map"
    goal_pose.target_pose.pose.position.x=pose[0][0]
    goal_pose.target_pose.pose.position.y=pose[0][1]
    goal_pose.target_pose.pose.position.z=pose[0][2]

    # r, p, y  欧拉角转四元数
    x,y,z,w=tf.transformations.quaternion_from_euler(pose[1][0],pose[1][1],pose[1][2])

    goal_pose.target_pose.pose.orientation.x=x
    goal_pose.target_pose.pose.orientation.y=y
    goal_pose.target_pose.pose.orientation.z=z
    goal_pose.target_pose.pose.orientation.w=w
    return goal_pose


if __name__ == "__main__": 
    #节点初始化
    rospy.init_node('patrol')

    #创建MoveBaseAction client
    client=actionlib.SimpleActionClient('move_base',MoveBaseAction)
    #等待MoveBaseAction server启动
    client.wait_for_server()

    while not rospy.is_shutdown():
        for pose in waypoints:
            goal=goal_pose(pose)
            client.send_goal(goal)
            client.wait_for_result()

下面是巡逻时的动图：