< >
Home » ROS机器人Diego制作 » ROS机器人Diego制作23-搭载EAI F4激光雷达move_base路径规划

ROS机器人Diego制作23-搭载EAI F4激光雷达move_base路径规划

ROS机器人Diego制作23-搭载EAI F4激光雷达move_base路径规划

说明:

  • 介绍F4激光雷达通过move_base进行路径规划
  • 基于前几篇博文,机器人已经可以创建地图,并可以实现定位,对于SLAM最后一部分就是针对进行导航路径规划
  • 这里我们要用到move_base包,导航其实就是机器人,在已有的地图上,借助amcl定位自己的位置
  • 并根据激光雷达数据避开地图上的障碍物,具体详细的理论可以查看官方的数据,或者baidu之前大神们的作品。

launch文件

  • 我们首先来看下配置好的launch文件。
  • 编辑文件diego_run_gmapping_amcl_flashgo.launch
  • 代码如下:
<launch>
  <master auto="start"/>

  <include file="$(find flashgo)/launch/lidar.launch" />

  <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
  </node>  

  <node pkg="tf" type="static_transform_publisher" name="base_frame_2_laser_link" args="0.0 0.0 0.2 3.14 3.14 0 /base_link /laser 40"/>   

  <!-- Map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find diego_nav)/maps/f4_gmapping_north.yaml" /> 

  <!-- amcl node -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
  <remap from="scan" to="scan"/>
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="initial_pose_x" value="0.0"/>
  <param name="initial_pose_y" value="0.0"/>
  <param name="initial_pose_a" value="0.0"/>
  <param name="use_map_topic" value="true"/>
  <param name="odom_model_type" value="diff"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.5" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="300"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.1"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.1"/>
  <param name="odom_alpha2" value="0.1"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.1"/>
  <param name="odom_alpha4" value="0.1"/>
  <param name="laser_z_hit" value="0.9"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
  <param name="laser_min_range" value="1"/>
  <param name="laser_max_range" value="8"/>
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>


  </node>

  <!-- move_base node -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find diego_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find diego_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find diego_nav)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find diego_nav)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find diego_nav)/config/base_local_planner_params.yaml" command="load" />
  </node>

</launch>
  • launch文件中amcl之前的内容在前面的博文中都已经讲解过了,这里主要看下move_base部分。

  • move_base节点的启动主要依赖于四个配置文件,所有的参数都需要在这四个文件中设置:

    • costmap_common_params.yaml代价地图通用配置文件
    • global_costmap_params.yaml全局代价地图配置文件
    • local_costmap_params.yaml本地代价地图配置文件
    • base_local_planner_params.yaml本地规划器配置文件
  • 我们接下来一个一个来看这四个文件:

(1)costmap_common_params.yaml

  • 这里为了说明方便直接在代码中注释说明,如果实际使用请把中文注释删掉,否则会出差
  • 代码如下:
obstacle_range: 2.5  #最大障碍物检测范围
raytrace_range: 3.0  #检测自由空间的最大范围
footprint: [[0.14, 0.14], [0.14, -0.14], [-0.14, 0.14], [-0.14, -0.14]]  #机器人为矩形,设置机器的在坐标系内所占用的面积
inflation_radius: 0.55  #与障碍物的安全系数

observation_sources: laser_scan_sensor #只关注激光雷达的数据

laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true} #设定激光雷达的相关参数

(2)global_costmap_params.yaml

  • 代码如下:
global_costmap:  
 global_frame: /map  #指定全局代价地图参考系为/map,
 robot_base_frame: base_link  #指定机器人的base_frame为base_link
 update_frequency: 5.0  #指定地图更新频率
 static_map: true #指定使用静态地图,并初始化
 transform_tolerance: 0.8 #指定tf的更新容忍度为0.8,可以根据硬件的实际情况调整这个参数,在树莓派下小于0.8会不断的报tf发布超时的警告信息

(3)local_costmap_params.yaml

  • 代码如下:
local_costmap:  
 global_frame: odom  #指定本地代价地图参考系为odom
 robot_base_frame: base_link  #指定机器人的base_frame为base_link
 update_frequency: 5.0  #指定地图更新频率
 publish_frequency: 2.0 #代价地图发布可视化信息的频率
 static_map: false  #本地代价地图会不断的更新地图,所以这里设置为false
 rolling_window: true  #设置滚动窗口,使得机器人始终在窗体中心位置
 width: 4.0  #代价地图的宽度
 height: 6.0  #代价地图的长度
 resolution: 0.05 #代价地图的分辨率

(4)base_local_planner_params.yaml

  • 这个文件中主要定义了,机器人控制的参数
  • 代码如下:
TrajectoryPlannerROS:  
  max_vel_x: 0.45  #x轴方向最大速度
  min_vel_x: 0.1  #x轴方向最小速度
  max_vel_theta: 1.0  #最大角速度
  min_in_place_vel_theta: 0.4  

  acc_lim_theta: 3.2  #最大角加速度
  acc_lim_x: 2.5  #x轴方向最大加速度
  acc_lim_y: 2.5  #y轴方向最大加速度

导航

  • 配置文件设置好后,就可以launch开始介绍的启动文件了,这里要注意的也是要设置机器人的起始位置
  • 机器人放置的位置也要和设置的起始位置一致
  • 执行命令:
roslaunch diego_nav diego_run_gmapping_amcl_flashgo.launch

测试导航

  • 我们可以导航测试代码中的几个点,改我为自己地图中的实际地址
  • 代码如下:
#!/usr/bin/env python  

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('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['point-0'] = Pose(Point(0.0, 2.0, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))  
        locations['point-1'] = Pose(Point(0.5, 2.0, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))  


        # Publisher to manually control the robot (e.g. to stop it)  
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  

        # 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.")
  • 经测试基本上机器人可以避开障碍物,但如果是在狭小的空间,机器会不停的打转,而没有办法出来,这可能与设定的参数有关,要达到好的效果还需要不断的调试参数,这里只是介绍了如何让机器人实现基本的路径规划。

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros机器人diego制作