Lesson 11: 导航-定位 & 路径规划

在已有地图的情况下,需要让机器人能够在地图中定位的自己的位置,这就需要时用到ROS的acml包来实现,同时发布目标位置通过move_base来做路径规划,绕过障碍物到达目的地。

1.定位

1.1 编写acml的launch启动脚本

<launch>
<master auto="start"/>

<include file="$(find rplidar_ros)/launch/rplidar.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.025 0.02 0.095 0.0 0.0 0 /base_link /laser 50"/>

<!-- Map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find diego_nav)/maps/rplidar_gmapping.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>
</launch>

这里需要强调的一点是必须配置机器人的其实位置点 ,机器人随便放一个位置,amcl并不会定位到,需要指定初始位置,地图的原点是用gmapping/hector构建地图时候的起始点,所以在launch文件中需要设定初始位置,我们在这里就把机器人放在起始的位置,的值都设定为0.0。

<param name="initial_pose_x" value="0.0"/>
<param name="initial_pose_y" value="0.0"/>

1.2 启动acml节点

roslaunch diego_nav diego_run_gmapping_amcl_flashgo.launch

打开新的终端,启动键盘控制节点

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

这时我们可以用键盘控制机器人动起来,并打开rviz查看定位情况

rosin rviz rviz

这张图中我们可以看到大量的红色箭头暨蒙特卡洛定位算法所产生的粒子分布,其中箭头方向是机器人运动的方向

这张图中可以看粒子比上张图更加聚合,从实际观察来看聚合度高的情况下定位效果更加。

2.路径规划

路径规划暨如何让机器人能够自主的从一个位置移动另外一个位置,途中可以自主避开障碍物,也是通常意义上导航的最直观体现,在ROS中路径规划是通过move_base这个包来实现的。

2.1 编写move _base的launch启动脚本

<launch>

<!-- 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文件内容也可以与acml的启动脚本合并在一个文件中。
move_base节点的配置参数分布在四个配置文件:
  • costmap_common_params.yaml
  • global_costmap_params.yaml
  • local_costmap_params.yaml
  • base_local_planner_params.yaml

2.2 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.3 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发布超时的警告信息

 

2.4 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 #代价地图的分辨率

 

2.5 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轴方向最大加速度

2.6启动move_base节点

roslaunch diego_nav diego_run_gmapping_amcl_flashgo.launch
此时Diego 1#已经具备了定位,导航的功能,接下来我们进行导航测试

3.导航测试

导航可以在室内固定位置放置障碍物,使用gmapping或者hector绘制有障碍物的地图,然后用上述的方法启动acml和move_base进行定位和导航。

3.1导航测试代码

我们可以修改导航测试代码,将其中的位置改为实际地图上的位置

#!/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.")

3.2启动导航

rosrun diego_nav nav_test.py

启动后,机器人将按照导航测试文件中所设置位置点逐个移动,并可以避开地图中的障碍物,但如果是在狭小的空间,机器会不停的打转,而没有办法出来,这可能与设定的参数有关,要达到好的效果还需要不断的调试参数。

发表评论

Scroll to top
%d 博主赞过: