SLAM导航 MoveBase
MoveBase: ROS导航逻辑的管理者
维护一张全局地图（基本上是不会更新的，一般是静态 costmap 类型），维护一张局部地图（实时更新，costmap 类型），维护一个全局路径规划器 global_planner 完成全局路径规划的任务， 维护一个局部路径规划器 base_local_planner 完成局部路径规划的任务。然后提供一个对外的服务，负责监听 nav_msgs::goal 类型的消息，然后调动全局规划器规划全局路径，再将全局路径送进局部规划器，局部规划器结合周围障碍信息（从其维护的 costmap 中查询）、全局路径信息、目标点信息采样速度并评分获得最高得分的轨迹（即是采样的最佳速度），然后返回速度值，由 move_base 发送 Twist 类型的 cmd_vel 消息上，从而控制机器人移动。完成导航任务。
代价地图: costmap_2d
通过激光或点云的数据，投影到 2D 平面上，创建代价地图，并可以设置膨胀半径。以层的概念来组织图层，默认的层有 static_layer（通过订阅 map_server 的/map主题）来生成， obstacle_layer 根据传感器获得的反馈来生成障碍物图层， inflation_layer则是将前两个图层的信息综合进行缓冲区扩展。
包括全局代价地图和局部代价地图
全局代价地图提供给全局路径规划器使用，生成全局路径
局部代价地图提供给局部路径规划器使用，生成局部路径与目标速度命令
全局路径规划机器：GlobalPlanner
局部路径规划器：LocalPlanner
teb-local-planner
dwa-local-planner
racecar_runway_navigation_dwa.launch
<?xml version="1.0"?>
<launch>
<!-- Launch the racecar -->
<include file="$(find racecar_gazebo)/launch/racecar.launch">
<arg name="world_name" value="racecar_runway"/>
</include>
<!-- Launch the built-map -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find racecar_gazebo)/map/map_runway.yaml" />
<!--Launch the move base with time elastic band-->
<param name="/use_sim_time" value="true"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find racecar_gazebo)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find racecar_gazebo)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find racecar_gazebo)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find racecar_gazebo)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find racecar_gazebo)/config/teb_local_planner_params.yaml" command="load" />
<rosparam file="$(find racecar_gazebo)/config/dwa_local_planner_params_dwa.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="0.01" />
<param name="planner_patience" value="5.0" />
<!--param name="use_dijkstra" value="false" /-->
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<!-- <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /> -->
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<param name="clearing_rotation_allowed" value="false" />
</node>
</launch>
dwa_local_planner_params_dwa.yaml
DWAPlannerROS:
max_vel_x: 1.0 # 机器人在x方向的最大速度 单位为m/s 0.55
min_vel_x: 0.4 # 机器人在x方向的最小速度 单位为m/s
max_vel_y: 0.0 # 机器人在y方向的最大速度 单位为m/s
min_vel_y: 0.0 # 机器人在y方向的最大速度 单位为m/s
max_vel_trans: 1.2 # 机器人最大平移速度的绝对值，单位为 m/s (default: 0.55) choose slightly less than the base's capability
min_vel_trans: 0.05 # 机器人最小平移速度的绝对值，单位为 m/s (default: 0.1) this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.1 #机器人被认属于“停止”状态时的平移速度。如果机器人的速度低于该值，则认为机器人已停止。单位为 m/s
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_vel_theta: 1.0 # 机器人的最大角速度的绝对值，单位为 rad/s (default: 1.0) choose slightly less than the base's capability
min_vel_theta: 0.1 # 机器人的最小角速度的绝对值，单位为 rad/s (default: 0.4) this is the min angular velocity when there is negligible translational velocity
theta_stopped_vel: 0.1 # 机器人被认属于“停止”状态时的旋转速度。单位为 rad/s
acc_lim_x: 18.0 # 机器人在x方向的加速度极限，单位为 meters/sec^2(default: 2.5) maximum is theoretically 2.0, but we
acc_lim_theta: 18.0 # 机器人的角加速度极限，单位为 radians/sec^2 。
acc_lim_y: 0.0 # 机器人在y方向的速度极限，单位为 meters/sec^2 diff drive robot
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.8 # 当实现其目标时，控制器在偏航/旋转方面的公差，单位为弧度。(default: 0.05) 0.8
xy_goal_tolerance: 0.1 # 当实现一个目标时，控制器在x和y距离上的公差，以米为单位。(default: 0.10) 0.3
latch_xy_goal_tolerance: false #如果锁定目标公差且机器人到达目标xy位置，机器人将简单地旋转到位，即使它在目标公差的范围内结束。
# Forward Simulation Parameters
sim_time: 1.7 # 前向模拟轨迹的时间，单位为 seconds (default: 1.7)
vx_samples: 8 # x方向速度的样本数 (default: 3)
vy_samples: 1 # y方向速度的样本数。(default: 10) diff drive robot, there is only one sample
vtheta_samples: 20 # 角速度的样本数。 (default: 20)
# Trajectory Scoring Parameters
path_distance_bias: 40.0 # 控制器靠近给定路径的权重 (default: 32.0) - weighting for how much it should stick to the global path plan
goal_distance_bias: 30.0 # 控制器应该在多大程度上尝试达到其本地目标的加权，也控制着速度。 (default: 24.0) - wighting for how much it should attempt to reach its goal
occdist_scale: 0.6 # 控制器尝试避免障碍物的权重。 (default: 0.01) - weighting for how much the controller should avoid obstacles
forward_point_distance: 0.2 # 从机器人中心点到放置一个额外得分点的距离，单位是米。 (default: 0.325) - how far along to place an additional scoring point
stop_time_buffer: 0.2 # 机器人在碰撞前必须停止的时间，以使轨迹被认为是有效的，单位是秒。(default: 0.2) - amount of time a robot must stop in before colliding for a valid traj.
scaling_speed: 0.25 # 开始缩放机器人足迹的速度的绝对值，单位是m/s。 (default: 0.25) - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # 缩放机器人足迹的最大系数为 (default: 0.2) - how much to scale the robot's footprint when at speed.
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05 # 机器人必须运动多少米远后才能复位震荡标记。(default: 0.05) - how far to travel before resetting oscillation flags
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: odom
# Differential-drive robot configuration - necessary?
# holonomic_robot: false
#The tolerance in radians for the controller in yaw/rotation when achieving its goal
#The tolerance in meters for the controller in the x & y distance when achieving a goal
latch_xy_goal_tolerance: false
#If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so.
处理cmd_vel数据
navigation_input_test.py
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Path, Odometry
from ackermann_msgs.msg import AckermannDriveStamped
from geometry_msgs.msg import PoseStamped, PoseArray, Twist
import math
import numpy as np
from numpy import linalg as LA
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import csv
import os
class VelPuber:
def __init__(self):
self.navigation_input = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=1)
self.cmd_vel_suber = rospy.Subscriber('/cmd_vel',Twist,self.cmd_vel_callback,queue_size = 5)
self.linear_vel = 0.0
self.angular_vel = 0.0
def vel_pub(self):
ackermann_control = AckermannDriveStamped()
ackermann_control.drive.speed = self.linear_vel
if self.linear_vel == 0:
ackermann_control.drive.steering_angle = 0
else:
ackermann_control.drive.steering_angle = math.atan(self.angular_vel/self.linear_vel)
ackermann_control.drive.steering_angle_velocity = self.angular_vel
self.navigation_input.publish(ackermann_control)
def cmd_vel_callback(self, data):
self.linear_vel = data.linear.x
self.angular_vel = data.angular.z
if __name__ == "__main__":
rospy.init_node("navation_input_test")
my_velpuber = VelPuber()
while not rospy.is_shutdown():
my_velpuber.vel_pub()
rospy.sleep(0.05)
任务
成功启动
racecar_navigation.launch（补全缺失的包）使用
rqt_reconfigure调整参数，在rivz中观察效果rosrun rqt_reconfigure rqt_reconfigure
编写
launch文件与yaml文件，配置dwa-local-planner相关参数，成功启动dwa-local-planner