pioneer3at完全自动导航

这个博客是之前三个move_base, gmapping和amcl的一个综合应用,如果前面三个关于pioneer3at的教程小伙伴们也都能够实现的话,这个实验应该也就不会太困难了,毕竟,万事开头难嘛…LZ可能还在头上O(∩_∩)O哈哈~

这个需要一个python的代码,在felaim_2dnav新建一个文件夹nodes,里面添加一个Python的脚本,nav_test.py

#!/usr/bin/env python
#因为有中文啦,要加这一句,不然会报错,不想添加也可以把中文注释去掉就行了
# coding=utf-8

#导入对应的依赖项
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?
    #设置在每一个目标位置向下一个目标位置移动前需要暂停的秒数
    #如果fake_test参数的值为true,那么rest_time参数会被忽略        
    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
    #可以用人类可读形式的MovaBaseAction目标状态很好的
        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()
        #这个是LZ通过gmapping建图后,在rviz上确定的三个目标位置
        locations['teacher_office'] = Pose(Point(-7.755, -21.776, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
        locations['elevator'] = Pose(Point(3.669, -23.923, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
        locations['student_office'] = Pose(Point(2.932, -15.759, 0.000), Quaternion(0.000, 0.000, -0.563, 0.826))


        # 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
                #用sample()函数从目标位置集合中生成随机的位置序列
                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
            # 设置机器人从一个位置到另一个位置,并发送它到move_base行为服务器
            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.")

开始在终端中运行对应的节点
第一步:

roscore

第二步:

//连接自己的机器人
roslaunch felaim_2dnav felaim_robot.launch

第三步:

//连接kinect
roslaunch felaim_2dnav pioneer3at_fake_laser_freenect.launch

第四步:

roslaunch felaim_2dnav tb_nav_test.launch

第五步:

rosrun rviz rviz -d `rospack find felaim_2dnav`/launch/nav_test.rviz

第六步

//监听数据
rqt_console &

tb_nav_test.launch

<launch>

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

  <!-- EDIT THIS LINE TO REFLECT THE NAME OF YOUR OWN MAP FILE 
       Can also be overridden on the command line -->
  <arg name="map" default="my_map.yaml" />

  <!-- Run the map server with the desired map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find felaim_2dnav)/maps/$(arg map)"/>

  <!-- Start the move_base node with the nav_test parameters -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find felaim_2dnav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find felaim_2dnav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find felaim_2dnav)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find felaim_2dnav)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find felaim_2dnav)/config/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find felaim_2dnav)/config/nav_test_params.yaml" command="load" />
  </node>


  <!-- Fire up AMCL -->
  <include file="$(find felaim_2dnav)/launch/tb_amcl.launch" />

  <!-- Start the navigation test -->
  <node pkg="felaim_2dnav" type="nav_test.py" name="nav_test" output="screen">
    <param name="rest_time" value="10" />
    <param name="fake_test" value="false" />
  </node>

</launch>

上图是终端打出的INFO,可以看出两次success的信息,目标位置也是在之前python文件中定义是位置列表.但LZ发现还是会有一些问题,如果障碍物不在地图上,那机器人就会有些迷糊…路径规划有时候会出地图…但是基本的功能是可以实现的,加上最近下雨,走廊里全是雨伞,跑的结果就是一般~(≧▽≦)/~啦啦啦,后续还得考虑下怎么进行修改^_^