文章目录
前言
??之前的文章中，介绍了如何创建地图和创建自定义地图，当时我是使用键盘控制机器人运动的，而后续实现了机器人的自主定位和导航。那么，在导航避障过程中，能不能同时实现地图创建呢？答案当然是肯定的。
??从本篇开始，使用RoboWare Studio进行开发。
一、创建功能包
??在robo_catkin_ws工作空间下，使用RoboWare Studio创建mbot_slam_navigation功能包，并完成依赖添加等工作，将mbot_sim_gazebo_navigation功能包中的部分文件复制到新功能包下。
二、自主探索SLAM
??本篇继续使用之前自定义的移动机器人模型和我家户型图，主要介绍两种方式实现自主探索SLAM，以gmapping为例（其它SLAM功能包类似）。
方式一、通过rviz设置探索目标
1、修改robot_gazebo_navigation.launch文件，如下：
<launch>
<arg name="paused" default="true"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<remap from="robot/laser/scan" to="/scan"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mbot_slam_navigation)/urdf/xacro/mbot_sim_gazebo_navigation.world"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
</include>
<!--机器人参数设置-->
<arg name="model" default="$(find mbot_slam_navigation)/urdf/xacro/robot.xacro" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model robot -param robot_description -z 0.05"/>
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_slam_navigation)/urdf/xacro/default.rviz"/>
<node name="slam_gmapping" pkg="gmapping" type="slam_gmapping">
<remap from="scan" to="/scan"/>
<param name="base_link" value="base_footprint"/>
</node>
</launch>
2、修改robot_gazebo_navigation_move.launch文件，如下：
<?xml version="1.0"?>
<launch>
<include file="$(find amcl)/examples/amcl_diff.launch">
</include>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0"/>
<param name="controller_patiente" value="15.0"/>
<rosparam file="$(find mbot_slam_navigation)/launch/costmap_common_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find mbot_slam_navigation)/launch/costmap_common_params.yaml" command="load" ns="local_costmap"/>
<rosparam file="$(find mbot_slam_navigation)/launch/local_costmap_params.yaml" command="load"/>
<rosparam file="$(find mbot_slam_navigation)/launch/global_costmap_params.yaml" command="load"/>
<rosparam file="$(find mbot_slam_navigation)/launch/base_local_planner_params.yaml" command="load"/>
</node>
</launch>
3、效果
依次运行如下命令：
roslaunch mbot_slam_navigation robot_gazebo_navigation.launch
roslaunch mbot_slam_navigation robot_gazebo_navigation_move.launch
gazebo和rviz效果如下：
??整个建图过程中，并不是像之前那样启动键盘控制节点，而是类似于导航功能的实现，使用rviz中2D Nav Goal工具，在rviz中选择一个导航的目标位置。
??确定目标后，机器人开始导航移动，同时使用gmapping实现地图的构建，运动过程中，move_base开始根据已经建立的地图和激光雷达信息，帮助机器人躲避周围不断出现的障碍物，不断的探索与目标点之间的路线。如果尝试多次，仍然无法到达目标点，机器人会放弃执行，并进行报错。
??建图完成后，切换到map文件夹下，执行如下命令：
rosrun map_server map_saver -f myHomeMapFile
??保存地图到map文件夹下。可以看到，建图效果与之前基本一样。
方式二、通过代码设置关键点
??方式一中的方法，需要不断点击目标位置，引导机器人完成SLAM，即需要手动操作。下面介绍另一种方法，不需要人工点击目标点，而是通过代码设置关键点的方式，让机器人随机导航，从而实现自主探索SLAM。
1、创建自主导航脚本文件
??使用RoboWare Studio，新建scrip文件夹，在此文件夹下，新建random_navigation.py文件，内容如下：
#! /usr/bin/env python
#coding:utf-8
import roslib;
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
from random import sample
from math import pow, sqrt
class NavTest():
def __init__(self):
rospy.init_node('random_navigation', anonymous=True)
rospy.on_shutdown(self.shutdown)
# 在每个目标位置暂停的时间
self.rest_time = rospy.get_param("~rest_time", 2)
# 到达目标的状态
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# 设置目标点的位置
# 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点
# 在终端中就会看到坐标信息
locations = dict()
locations['p1'] = Pose(Point(3.057, -4.795, 0.000), Quaternion(0.000, 0.000, 0.000, 0.168))
locations['p2'] = Pose(Point(2.495, -2.381, 0.000), Quaternion(0.000, 0.000, 0.063, 0.347))
locations['p3'] = Pose(Point(3.240, -0.014, 0.000), Quaternion(0.000, 0.000, 0.946, -2.675))
locations['p4'] = Pose(Point(2.336, 2.304, 0.000), Quaternion(0.000, 0.000, 0.139, -0.084))
locations['p5'] = Pose(Point(-0.831, 2.264, 0.000), Quaternion(0.000, 0.000, 0.919, -0.433))
locations['p6'] = Pose(Point(-3.523, 1.825, 0.000), Quaternion(0.000, 0.000, 0.627, 2.071))
locations['p7'] = Pose(Point(-4.002, 1.644, 0.000), Quaternion(0.000, 0.000, 0.139, -1.119))
locations['p8'] = Pose(Point(-6.420, -2.611, 0.000), Quaternion(0.000, 0.000, 0.919, -1.839))
locations['p9'] = Pose(Point(-6.420, -1.372, 0.000), Quaternion(0.000, 0.000, 0.627, 1.440))
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存机器人的在rviz中的初始位置
initial_pose = PoseWithCovarianceStamped()
# 保存成功率、运行时间、和距离的变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# 确保有初始位置
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 开始主循环，随机导航
while not rospy.is_shutdown():
# 如果已经走完了所有点，再重新开始排序
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# 如果最后一个点和第一个点相同，则跳过
if sequence[0] == last_location:
i = 1
# 在当前的排序中获取下一个目标点
location = sequence[i]
# 跟踪行驶距离
# 使用更新的初始位置
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# 存储上一次的位置，计算距离
last_location = location
# 计数器加1
i += 1
n_goals += 1
# 设定下一个目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# 让用户知道下一个位置
rospy.loginfo("Going to: " + str(location))
# 向下一个位置进发
self.move_base.send_goal(self.goal)
# 五分钟时间限制
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# 查看是否成功到达
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# 运行所用时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def update_initial_pose(self, initial_pose):
self.initial_pose = initial_pose
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def trunc(f, n):
slen = len('%.*f' % (n, f))
return float(str(f)[:slen])
if __name__ == '__main__':
try:
NavTest()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Random navigation finished.")
其中，P1-P9点分别对应如下9个坐标点：
P1：厨房
P2：餐厅
P3：卫生间
P4：次卧
P5：书房
P6：主卧点1
P7：主卧点2
P8：阳台点1
P9：阳台点2
2、效果
依次运行如下命令：
roslaunch mbot_slam_navigation robot_gazebo_navigation.launch
roslaunch mbot_slam_navigation robot_gazebo_navigation_move.launch
启动成功后，运行切换到scrip目录下，运行脚本文件：
rosrun mbot_slam_navigation random_navigation.py
gazebo和rviz效果如下：
??可以看到gazebo和rviz中的机器人很快就开始移动了，通过随机产生的目标点，机器人一边导航避障，一边完成SLAM建图。
??建图完成后，切换到map文件夹下，执行如下命令：
rosrun map_server map_saver -f myMapFile
??保存地图到map文件夹下。可以对比下myMapFile.pgm和myHomeMapFile.pgm文件，建图效果基本一样。
??在最初的未知世界的探索过程中，机器人由于不知道地图信息，所以路径规划往往不是最优路径，很多时候还会困在墙角，此时机器人会不断旋转并继续前进，也有可能找不到出路，认为目标不可到达，并报错，这就需要我们调整导航参数来优化机器人的行为。
??当地图建立完成后，后面的导航运动就很流畅了，move_base可以根据已经建立的地图和激光雷达信息，躲避障碍物，规划最优的路径。
