ROS机器人学习(八)--ROS机器人自主导航

导航仿真

上一篇博文已经介绍了如何配置move_base和amcl，但是大家应该还是对它们没有一个直观的感受和认识，现在我们来进行调用它们结合之前的知识来实现ROS机器人自主导航。

新建launch启动文件

在 mbot_navigation 功能包launch目录下新建启动文件，命名为nav_cloister_demo.launch。
此launch文件加载了地图配置，启动了move_base节点和AMCL节点：

<launch>

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="cloister_gmapping.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

    <!-- 启动AMCL节点 -->
    <include file="$(find mbot_navigation)/launch/amcl.launch" />

    <!-- 对于虚拟定位，需要设置一个/odom与/map之间的静态坐标变换 -->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>

</launch>

启动导航仿真

# 启动仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
# 启动导航节点
roslaunch mbot_navigation nav_cloister_demo.launch 

导航SLAM仿真

我们现在有通过gmapping功能包完成机器人的SLAM功能，也有通过导航来完成了到达目标点的路径规划。
接下来，我们就把SLAM和导航结合起来。
之前的激光SLAM建图，我们是通过键盘控制机器人在未知环境中运动来完成建图，这个过程当中人参与的部分是比较多的。现在我们结合导航功能，使机器人自主的去探索未知环境，来完成最终的地图构建。

新建launch启动文件

在 mbot_navigation 功能包launch目录下新建启动文件，命名为exploring_slam_demo.launch：

<launch>

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

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>

</launch>

获取坐标点

编写一个小程序，获取机器人在地图中的坐标点位置，命名为getRobotPos.py:

#!/usr/bin/env python
import rospy

from tf_conversions import transformations
from math import pi
import tf

class Robot:
    def __init__(self):
        self.tf_listener = tf.TransformListener()
        try:
            self.tf_listener.waitForTransform('/map', '/base_link', rospy.Time(), rospy.Duration(1.0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            return

    def get_pos(self):
        try:
            (trans, rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.loginfo("tf Error")
            return None
        euler = transformations.euler_from_quaternion(rot)
        #print euler[2] / pi * 180

        x = trans[0]
        y = trans[1]
        th = euler[2] / pi * 180
        return (x, y, th)

if __name__ == "__main__":
    rospy.init_node('get_pos_demo',anonymous=True)
    robot = Robot()
    r = rospy.Rate(100)
    r.sleep()
    while not rospy.is_shutdown():
        print robot.get_pos()
        r.sleep()

启动导航SLAM仿真

# 启动仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
# 启动SLAM+导航节点
roslaunch mbot_navigation exploring_slam_demo.launch
# 获取机器人坐标点
rosrun mbot_navigation getRobotPos.py

通过2D Nav Goal 选择目标点来控制机器人移动，机器人在运动过程中会把周围的环境地图一步步地构建出来。

自主探索SLAM仿真

编写一个小程序，控制机器人自主运动

在 mbot_navigation 功能包script目录下编写一个小程序，命名为exploring_slam.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('exploring_slam', anonymous=True)  
        rospy.on_shutdown(self.shutdown)  

        # 在每个目标位置暂停的时间 (单位：s)
        self.rest_time = rospy.get_param("~rest_time", 2)  

        # 是否仿真？  
        self.fake_test = rospy.get_param("~fake_test", True)  

        # 到达目标的状态  
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
                       'SUCCEEDED', 'ABORTED', 'REJECTED',  
                       'PREEMPTING', 'RECALLING', 'RECALLED',  
                       'LOST']  
 
        # 设置目标点的位置    
        locations = dict()  

        locations['1'] = Pose(Point(4.589, -0.376, 0.000),  Quaternion(0.000, 0.000, -0.447, 0.894))  
        locations['2'] = Pose(Point(4.231, -6.050, 0.000),  Quaternion(0.000, 0.000, -0.847, 0.532))  
        locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))  
        locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
        locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))  
        locations['6'] = Pose(Point(2.924, 0.018, 0.000),   Quaternion(0.000, 0.000, 0.000, 1.000))  

        # 发布控制机器人的消息  
        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("Exploring SLAM finished.")

启动自主探索SLAM仿真

# 启动仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
# 启动SLAM+导航节点
roslaunch mbot_navigation exploring_slam_demo.launch
# 运行一个小程序，控制机器人运动
rosrun mbot_navigation exploring_slam.py