ROS学习(十六)自定义机器人自主探索SLAM


前言

  之前的文章中,介绍了如何创建地图和创建自定义地图,当时我是使用键盘控制机器人运动的,而后续实现了机器人的自主定位和导航。那么,在导航避障过程中,能不能同时实现地图创建呢?答案当然是肯定的。
  从本篇开始,使用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可以根据已经建立的地图和激光雷达信息,躲避障碍物,规划最优的路径。

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
扎眼的阳光的头像扎眼的阳光普通用户
上一篇 2022年5月12日
下一篇 2022年5月12日

相关推荐