Ros學習——導航

1.導航框架

 

 

 在總體框架圖中可以看到，move_base提供了ROS導航的配置、運行、交互接口，它主要包括兩個部分：
      （1） 全局路徑規划（global planner）：根據給定的目標位置進行總體路徑的規划；
      （2） 本地實時規划（local planner）：根據附近的障礙物進行躲避路線規划。

• 全局路徑規划（global planner）

        在ROS的導航中，首先會通過全局路徑規划，計算出機器人到目標位置的全局路線。這一功能是navfn這個包實現的。
        navfn通過Dijkstra最優路徑的算法，計算costmap上的最小花費路徑，作為機器人的全局路線。將來在算法上應該還會加入A*算法。

• 本地實時規划（local planner）

        本地的實時規划是利用base_local_planner包實現的。該包使用Trajectory Rollout 和Dynamic Window approaches算法計算機器人每個周期內應該行駛的速度和角度（dx，dy，dtheta velocities）。

　　base_local_planner這個包通過地圖數據，通過算法搜索到達目標的多條路經，利用一些評價標准（是否會撞擊障礙物，所需要的時間等等）選取最優的路徑，並且計算所需要的實時速度和角度。
　　其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：
      （1） 采樣機器人當前的狀態（dx,dy,dtheta）；
      （2） 針對每個采樣的速度，計算機器人以該速度行駛一段時間后的狀態，得出一條行駛的路線。
      （3） 利用一些評價標准為多條路線打分。
      （4） 根據打分，選擇最優路徑。
      （5） 重復上面過程。

 

2.安裝rbx1 package

• 安裝gmapping： 由激光雷達數據生成地圖（或者深度相機數據）

git clone https://github.com/ros-perception/slam_gmapping.git 

• 安裝amcl：在已有的地圖內定位機器人

git clone https://github.com/ros-planning/navigation.git

• 其他可能用到的包：提示沒有哪個就裝哪個

sudo apt-get install 

ros-indigo-turtlebot-bringup \
ros-indigo-turtlebot-create-desktop ros-indigo-openni-* \
ros-indigo-openni2-* ros-indigo-freenect-* ros-indigo-usb-cam \
ros-indigo-laser-* ros-indigo-hokuyo-node \
ros-indigo-audio-common gstreamer0.10-pocketsphinx \
ros-indigo-pocketsphinx ros-indigo-slam-gmapping \
ros-indigo-joystick-drivers python-rosinstall \
ros-indigo-orocos-kdl ros-indigo-python-orocos-kdl \
python-setuptools ros-indigo-dynamixel-motor-* \

libopencv-dev python-opencv ros-indigo-vision-opencv \
ros-indigo-depthimage-to-laserscan ros-indigo-arbotix-* \（我安裝的過程中提示沒有這個： ERROR:cannot launch node of type [arbotix_python/arbotix_driver]:arbotix_python）
ros-indigo-turtlebot-teleop ros-indigo-move-base \
ros-indigo-map-server ros-indigo-fake-localization \
ros-indigo-amcl git subversion mercurial

• 安裝rbx1 package 

cd ~/catkin_ws/src
git clone https://github.com/pirobot/rbx1.git cd rbx1
git checkout indigo-devel
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
rospack profile

• 測試

roslaunch rbx1_bringup fake_turtlebot.launch
rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

 

提示錯誤：Error: package 'rbx1_nav' not found。vmw_ioctl_command error Invalid argument.

解決：退出后，在終端中運行：

$ export SVGA_VGPU10=0

 

 3.運行正方形

• 運行機器人

1.打開一個終端
cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src
roslaunch rbx1_bringup fake_turtlebot.launch

• 運行rviz仿真環境

//新打開一個終端

cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src
rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.rviz

• 運行空地圖

//新打開一個終端
cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src
roslaunch rbx1_nav fake_move_base_blank_map.launch 

• 運行正方形路徑 

//打開一個新終端，
cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src
rosrun rbx1_nav move_base_square.py  

代碼解釋 move_base_square.py  

 

4.避障行走

• 找到空白地圖的終端，ctrl+c退出這個地圖，運行障礙地圖

 roslaunch rbx1_nav fake_move_base_map_with_obstacles.launch

•  運行正方形路徑

rosrun rbx1_nav move_base_square.py  

 運行效果：

 

 若沒有顯示障礙物，在rviz里加載了這幾個display,其中global plan下的costmap，RobotModel,global plan下的Path，Axes是必須的。

 

 

5.amcl定位

• 先運行仿真機器人

//打開一個新終端
cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src
roslaunch rbx1_bringup fake_turtlebot.launch

• 運行amcl節點

amcl是二維環境下的概率定位系統，之所以說是概率定位系統，是因為它用的是自適應的蒙特卡洛的定位方法，就是之前的粒子濾波，用這個粒子濾波去跟蹤機器人當前的狀態/

//打開一個新終端

cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src 
roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml

• 然后運行rviz

cd ~/catkin_ws/
catkin_make
source ./devel/setup.bash
cd src
rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.rviz

 

 

 

 6.關鍵文件解析

 fake_nav_test.launch：

<launch>

  <param name="use_sim_time" value="false" />

  <!-- Start the ArbotiX controller -->
  <include file="$(find rbx1_bringup)/launch/fake_turtlebot.launch" />//加載機器人驅動

  <!-- Run the map server with the desired map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/test_map.yaml"/>//這里的地圖就是map_server,有時候可不需要

  <!-- The move_base node -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" clear_params="true" output="screen">//加載了它的幾個配置文件，分別是：
    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/nav_test_params.yaml" command="load" />
  </node>
  
  <!-- Run fake localization compatible with AMCL output -->
  <node pkg="fake_localization" type="fake_localization" name="fake_localization" clear_params="true" output="screen">//調用之前的amcl節點
     <remap from="base_pose_ground_truth" to="odom" />
     <param name="global_frame_id" value="map" />
     <param name="base_frame_id" value="base_footprint" />
  </node>
  
  <!-- Start the navigation test -->
  <node pkg="rbx1_nav" type="nav_test.py" name="nav_test" output="screen">//加載nav_test.py程序，進行隨機導航（這里的隨機是指目標點位置隨機）
    <param name="rest_time" value="1" />
    <param name="fake_test" value="true" />
  </node>
  
</launch>

 

nav_test.py

#!/usr/bin/env python

""" nav_test.py - Version 1.1 2013-12-20

    Command a robot to move autonomously among a number of goal locations defined in the map frame.
    On each round, select a new random sequence of locations, then attempt to move to each location
    in succession.  Keep track of success rate, time elapsed, and total distance traveled.

    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""

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('nav_test', anonymous=True)
        
        rospy.on_shutdown(self.shutdown)
        
        # How long in seconds should the robot pause at each location?
        self.rest_time = rospy.get_param("~rest_time", 10)
        
        # Are we running in the fake simulator?
        self.fake_test = rospy.get_param("~fake_test", False)
        
        # Goal state return values
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 
                       'SUCCEEDED', 'ABORTED', 'REJECTED',
                       'PREEMPTING', 'RECALLING', 'RECALLED',
                       'LOST']
        
        # Set up the goal locations. Poses are defined in the map frame.  
        # An easy way to find the pose coordinates is to point-and-click
        # Nav Goals in RViz when running in the simulator.
        # Pose coordinates are then displayed in the terminal
        # that was used to launch RViz.
        locations = dict()
        
        locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))
        locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
        locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))
        locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))
        locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
        locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
        
        # Publisher to manually control the robot (e.g. to stop it, queue_size=5)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))
        
        rospy.loginfo("Connected to move base server")
        
        # A variable to hold the initial pose of the robot to be set by 
        # the user in RViz
        initial_pose = PoseWithCovarianceStamped()
        
        # Variables to keep track of success rate, running time,
        # and distance traveled
        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 = ""
        
        # Get the initial pose from the user
        rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
        self.last_location = Pose()
        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
        
        # Make sure we have the initial pose
        while initial_pose.header.stamp == "":
            rospy.sleep(1)
            
        rospy.loginfo("Starting navigation test")
        
        # Begin the main loop and run through a sequence of locations
        while not rospy.is_shutdown():
            # If we've gone through the current sequence,
            # start with a new random sequence
            if i == n_locations:
                i = 0
                sequence = sample(locations, n_locations)
                # Skip over first location if it is the same as
                # the last location
                if sequence[0] == last_location:
                    i = 1
            
            # Get the next location in the current sequence
            location = sequence[i]
                        
            # Keep track of the distance traveled.
            # Use updated initial pose if available.
            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 = ""
            
            # Store the last location for distance calculations
            last_location = location
            
            # Increment the counters
            i += 1
            n_goals += 1
        
            # Set up the next goal location
            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()
            
            # Let the user know where the robot is going next
            rospy.loginfo("Going to: " + str(location))
            
            # Start the robot toward the next location
            self.move_base.send_goal(self.goal)
            
            # Allow 5 minutes to get there
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 
            
            # Check for success or failure
            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]))
            
            # How long have we been running?
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0
            
            # Print a summary success/failure, distance traveled and time elapsed
            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):
    # Truncates/pads a float f to n decimal places without rounding
    slen = len('%.*f' % (n, f))
    return float(str(f)[:slen])

if __name__ == '__main__':
    try:
        NavTest()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("AMCL navigation test finished.")

　

 7.執行步驟解析

• 執行fake_nav_test.launch

　　　　1.加載機器人驅動fake_turtlebot.launch

　　　　　　1.1 Load the URDF/Xacro model

　　　　　　1.2 加載arbotix節點（加載配置文件：fake_turtlebot_arbotix.yaml）

　　　　　　1.3 加載robot_state_publisher節點（設置頻率publish_frequency：20）

　　　　2. 加載地圖節點

　　　　3. 加載move_base節點（包含costmap_common_params.yaml；local_costmap_params.yaml；global_costmap_params.yaml；base_local_planner_params.yaml；nav_test_params.yaml四個配置文件）

　　　　4. 加載fake_localization節點（AMCL）

　　　　5. 加載rbx1_nav節點，調用nav_test.py程序進行導航

• 執行nav_test_fuerte.rviz

 

 參考：

https://www.cnblogs.com/talugirl/p/5962806.html

https://blog.csdn.net/hcx25909/article/details/9470297