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.")
- 经测试基本上机器人可以避开障碍物，但如果是在狭小的空间，机器会不停的打转，而没有办法出来，这可能与设定的参数有关，要达到好的效果还需要不断的调试参数，这里只是介绍了如何让机器人实现基本的路径规划。
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号