move_base导航：用C++代码走三角形、四边形、避障等
系统版本：Ubuntu14.04，ROS indigo
机器人底盘：kobuki
摄像头：Asus Xtion
编程语言：C++
参考书本：《ROS by Example》 《Learning ROS for Robotics Programming》
在上一篇博文，我们监控/odom坐标和/base_footprint坐标之间的tf变换，从而跟踪机器人移动的距离和旋转的角度。ROS提供更加简洁的方法即是使用move_base包也能实现一样的效果。其中move_base包使用ROS action工具来达到一个给定的导航目标点。当选择机器人行走的路径时，move_base包合并base_local_planner基本局部规划器、全局代价地图和局部代价地图的里程数据。
参考资料：
move_base Wiki：http://wiki.ros.org/move_base
actionlib Wiki：http://wiki.ros.org/actionlib
为了使用move_base制定一个导航目标，我们提供一个相对特定参考坐标的机器人的目标姿态（位置和方向）。move_base包使用MoveBaseActionGoal消息类型来指定目标。运行一下命令查看MoveBaseActionGoal消息类,如图1。
rosmsg show MoveBaseActionGoal
图1
可见，目标由标准ROS header、goal_id和goal本身组成。goal里的PoseStamped消息类型依次由header和pose组成，其中pose包括了position和orientation。
接下来使用move_base导航一个正方形。
创建功能包
catkin_create_pkg robot_navigation actionlib geometry_msgs move_base_msgs visualization_msgs roscpp tf创建机器人配置
在robot_navigation/launch文件夹下fake_move_base_blank_map.launch为名创建一个文件，并添加一下代码：
此.launch文件的解释：首先我们先运行带有参数blank_map.yaml空白地图的ROS map_server节点。.yaml文件描述地图本身的大小和分辨率。接下来的第二行命令包含可 fake_move_base.launch文件，此文件是运行move_base节点和加载所有必要配置参数以使仿真机器人工作得更好。最后，因为我们使用一个空白的地图和我们的仿真机器人没有传感器，机器人无法为定位获取扫描数据。取而代之，假设量程是正确的，我们简单设置一个静态身份转换，使用机器人的量程坐标转换成地图map坐标。
以下是fake_move_base.launch文件内容：
此launch文件运行move_base节点并五次回调rosparam加载.yaml文件。加载两次costmap_common_params.yaml文件的原因是使用global_costmap命名空间和local_costmap命名空间来设置那些共同参数。
参数配置
根据fake_move_base.launch文件内容，在robot_navigation/config/fake文件夹下分别创建costmap_common_params.yaml，local_costmap_params.yaml，global_costmap_params.yaml，base_local_planner_params.yaml这四个配置文件。
costmap_common_params.yaml文件内容如下：
obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.175
inflation_radius: 0.2
max_obstacle_height: 0.6
min_obstacle_height: 0.0
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
这个文件主要用于配置基本参数。这些参数会被用于local_costmap和global_costmap。obstacle_range和raytrace_range参数用于表示传感器的最大探测距离并在代价地图中引入探测的障碍物。前者主要用于障碍的探测。在此示例中，如果机器人检测到一个距离小于2.5的障碍物，就会将这个障碍物引入到代价地图中。后者则用于在机器人运动过程中，实时清除代价地图中的障碍物，并更新可移动的自由空间数据。
robot_radius表示机器人的半径大小。inflation_radius表示机器人与障碍物之间必须保持的最小距离。max_obstacle_height表示传感器读取最大高度被视为有效，如果此参数的值比全局max_obstacle_height的值低，则会过滤掉此高度以上的点。min_obstacle_height表示传感器读取最小高度被视为有效。observation_sources设定导航功能包集所使用的传感器。
local_costmap_params.yaml文件内容如下：
local_costmap:
global_frame: /odom
robot_base_frame: /base_footprint
update_frequency: 3.0
publish_frequency: 1.0
static_map: true
rolling_window: false
width: 6.0
height: 6.0
resolution: 0.01
transform_tolerance: 1.0global_frame: /odom表示对于局部代价地图，使用量程坐标作为全局坐标。对于TurtleBot或者kobuki的机器人参考坐标robot_base_frame是/base_footprint。update_frequency表示基于传感器数据更新局部地图的频率。publish_frequency表示发布信息的频率。rolling_window用于配置在机器人运动过程中，代价地图始终以机器人为中心。width、height和resolution参数来配置代价地图的尺寸和分辨率，以米为单位。transform_tolerance表示在tf树坐标之间转换的允许延时或者绘图过程临时被中止的允许延时，以秒为单位。
global_costmap_params.yaml文件内容如下：
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 1.0
publish_frequency: 1.0
static_map: true
rolling_window: false
resolution: 0.01
transform_tolerance: 1.0
map_type: costmapglobal_frame: /map表示对于全局代价地图，使用/map作为全局坐标。map_type有两种类型：“voxel”和“costmap”，“voxel”表示3D视图，“costmap”表示2D视图。
base_local_planner_params.yaml文件内容如下：
controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false
TrajectoryPlannerROS:
max_vel_x: 0.5
min_vel_x: 0.1
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.4
escape_vel: -0.1
acc_lim_x: 1.5
acc_lim_y: 0.0 # zero for a differential drive robot
acc_lim_theta: 1.2
holonomic_robot: false
yaw_goal_tolerance: 0.2 # about 6 degrees
xy_goal_tolerance: 0.2 # 5 cm
latch_xy_goal_tolerance: false
pdist_scale: 0.4
gdist_scale: 0.8
meter_scoring: true
heading_lookahead: 0.325
heading_scoring: false
heading_scoring_timestep: 0.8
occdist_scale: 0.05
oscillation_reset_dist: 0.05
publish_cost_grid_pc: false
prune_plan: true
sim_time: 1.0
sim_granularity: 0.05
angular_sim_granularity: 0.1
vx_samples: 8
vy_samples: 0 # zero for a differential drive robot
vtheta_samples: 20
dwa: true
simple_attractor: false
controller_frequency表示更新规划进程的频率，每秒多少次。max_vel_x表示机器人最大线速度，单位是m/s。min_vel_x表示机器人最小线速度。max_vel_y和min_vel_y对于两轮差分驱动机器人是无效的。max_vel_theta表示机器人最大转动速度，单位是rad/s。min_vel_theta表示机器人最小转动速度。min_in_place_vel_theta表示机器人最小原地旋转速度。escape_vel表示机器人的逃离速度，即背向相反方向行走速度，单位是m/s。acc_lim_x表示在x方向上最大的线加速度，单位是m/s^2。 acc_lim_theta表示最大角加速度，单位是rad/s^2。holonomic_robot：除非拥有一个全方位的机器人，否则设置为false。yaw_goal_tolerance表示接近目标方向（就弧度来说）允许的误差（rad），此值太小会造成机器人在目标附近震荡。xy_goal_tolerance表示接近目标允许的误差（m），此值设置太小，机器人会没完没了在目标位置周围做小调整，而且此值不能小于地图的分辨率。pdist_scale表示紧贴全局路径比到达目标的相对重要性，如果此值比gdist_scale大，那么机器人会更紧靠全局路径行走。gdist_scale表示到达目标比紧靠全局路径的相对重要性，如果此值比pdist_scale大，那么机器人会采取任意路径优先到达目标。meter_scoring表示gdist_scale和pdist_scale参数是否假设goal_distance和path_distance以米或者单元来表达。occdist_scale表示控制器尝试避开障碍物的权重。sim_time表示规划器能看到未来多少秒。dwa表示当模拟轨迹走向未来，是否使用动态窗口法。
在robot_navigation/launch文件夹创建名为view_nav.launch，用于启动rviz，内容如下：
- 使用move_base导航一个正方形
1.在robot_navigation/src文件夹下创建move_base_square.cpp为名的文件，并复制以下代码：
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
ros::Publisher cmdVelPub;
ros::Publisher marker_pub;
void shutdown(int sig)
{
cmdVelPub.publish(geometry_msgs::Twist());
ros::Duration(1).sleep(); // sleep for a second
ROS_INFO("nav_square.cpp ended!");
ros::shutdown();
}
void init_markers(visualization_msgs::Marker *marker)
{
marker->ns = "waypoints";
marker->id = 0;
marker->type = visualization_msgs::Marker::CUBE_LIST;
marker->action = visualization_msgs::Marker::ADD;
marker->lifetime = ros::Duration();//0 is forever
marker->scale.x = 0.2;
marker->scale.y = 0.2;
marker->color.r = 1.0;
marker->color.g = 0.7;
marker->color.b = 1.0;
marker->color.a = 1.0;
marker->header.frame_id = "odom";
marker->header.stamp = ros::Time::now();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "nav_move_base");
std::string topic = "/cmd_vel";
ros::NodeHandle node;
//Subscribe to the move_base action server
actionlib::SimpleActionClient ac("move_base",true);
//Define a marker publisher.
marker_pub = node.advertise("waypoint_markers", 10);
//for init_markers function
visualization_msgs::Marker line_list;
signal(SIGINT, shutdown);
ROS_INFO("move_base_square.cpp start...");
//How big is the square we want the robot to navigate?
double square_size = 1.0;
//Create a list to hold the target quaternions (orientations)
geometry_msgs::Quaternion quaternions[4];
//convert the angles to quaternions
double angle = M_PI/2;
int angle_count = 0;
for(angle_count = 0; angle_count < 4;angle_count++ )
{
quaternions[angle_count] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, angle);
angle = angle + M_PI/2;
}
//a pose consisting of a position and orientation in the map frame.
geometry_msgs::Point point;
geometry_msgs::Pose pose_list[4];
point.x = square_size;
point.y = 0.0;
point.z = 0.0;
pose_list[0].position = point;
pose_list[0].orientation = quaternions[0];
point.x = square_size;
point.y = square_size;
point.z = 0.0;
pose_list[1].position = point;
pose_list[1].orientation = quaternions[1];
point.x = 0.0;
point.y = square_size;
point.z = 0.0;
pose_list[2].position = point;
pose_list[2].orientation = quaternions[2];
point.x = 0.0;
point.y = 0.0;
point.z = 0.0;
pose_list[3].position = point;
pose_list[3].orientation = quaternions[3];
//Initialize the visualization markers for RViz
init_markers(&line_list);
//Set a visualization marker at each waypoint
for(int i = 0; i < 4; i++)
{
line_list.points.push_back(pose_list[i].position);
}
//Publisher to manually control the robot (e.g. to stop it, queue_size=5)
cmdVelPub = node.advertise(topic, 5);
ROS_INFO("Waiting for move_base action server...");
//Wait 60 seconds for the action server to become available
if(!ac.waitForServer(ros::Duration(60)))
{
ROS_INFO("Can't connected to move base server");
return 1;
}
ROS_INFO("Connected to move base server");
ROS_INFO("Starting navigation test");
//Initialize a counter to track waypoints
int count = 0;
//Cycle through the four waypoints
while( (count < 4) && (ros::ok()) )
{
//Update the marker display
marker_pub.publish(line_list);
//Intialize the waypoint goal
move_base_msgs::MoveBaseGoal goal;
//Use the map frame to define goal poses
goal.target_pose.header.frame_id = "map";
//Set the time stamp to "now"
goal.target_pose.header.stamp = ros::Time::now();
//Set the goal pose to the i-th waypoint
goal.target_pose.pose = pose_list[count];
//Start the robot moving toward the goal
//Send the goal pose to the MoveBaseAction server
ac.sendGoal(goal);
//Allow 1 minute to get there
bool finished_within_time = ac.waitForResult(ros::Duration(180));
//If we dont get there in time, abort the goal
if(!finished_within_time)
{
ac.cancelGoal();
ROS_INFO("Timed out achieving goal");
}
else
{
//We made it!
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Goal succeeded!");
}
else
{
ROS_INFO("The base failed for some reason");
}
}
count += 1;
}
ROS_INFO("move_base_square.cpp end...");
return 0;
}
2.修改robot_navigation目录下的CMakeLists.txt
在CMakeLists.txt文件末尾加入几条语句:
add_executable(move_base_square src/move_base_square.cpp)
target_link_libraries(move_base_square ${catkin_LIBRARIES})
3.编译程序
在catkin_ws目录下，执行一下命令编译：
catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles" -DCMAKE_BUILD_TYPE=Debug -DCMAKE_ECLIPSE_MAKE_ARGUMENTS=-j8
得到move_base_square执行程序。
为何不直接执行catkin_make编译？因为执行命令也可以让Eclipse识别该工程，方便在Eclipse编写调试代码。执行过一次，以后只需catkin_make编译，Eclipse也能识别。
4.测试程序
4.1 启动roscore
roscore- 4.2 启动仿真机器人
roslaunch aicroboxi_bringup fake_aicroboxi.launch4.3 启动带有空白地图的map_server节点，启动move_base节点并加载全局和局部代价地图等配置文件
roslaunch robot_navigation fake_move_base_blank_map.launch
4.4 启动 rviz 图形化显示程序
roslaunch robot_navigation view_nav.launchview_nav.launch的内容如下：
4.5 启动move_base_square程序，效果如图2
rosrun robot_navigation move_base_square
部分代码解释
//convert the angles to quaternions
double angle = M_PI/2;
int angle_count = 0;
for(angle_count = 0; angle_count < 4;angle_count++ )
{
quaternions[angle_count] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, angle);
angle = angle + M_PI/2;
}
在正方形四个角定义了目标方向。分别将四个角度值转换成四元数。
/a pose consisting of a position and orientation in the map frame.
geometry_msgs::Point point;
geometry_msgs::Pose pose_list[4];
point.x = square_size;
point.y = 0.0;
point.z = 0.0;
pose_list[0].position = point;
pose_list[0].orientation = quaternions[0];
point.x = square_size;
point.y = square_size;
point.z = 0.0;
pose_list[1].position = point;
pose_list[1].orientation = quaternions[1];
point.x = 0.0;
point.y = square_size;
point.z = 0.0;
pose_list[2].position = point;
pose_list[2].orientation = quaternions[2];
point.x = 0.0;
point.y = 0.0;
point.z = 0.0;
pose_list[3].position = point;
pose_list[3].orientation = quaternions[3];
创建四个航点的姿势，包括四个角的坐标和方向。
//Initialize the visualization markers for RViz
init_markers(&line_list);
//Set a visualization marker at each waypoint
for(int i = 0; i < 4; i++)
{
line_list.points.push_back(pose_list[i].position);
}在RViz上设置虚拟标记是相当简单的，教程地址：http://wiki.ros.org/rviz/Tutorials
此例程在每个目标角落放置四个红色矩形。init_markers(&line_list)此函数在程序的前面有定义，主要设置标记形状、大小、和颜色。然后line_list列表存放四个矩形的坐标。
//Subscribe to the move_base action server
actionlib::SimpleActionClient ac("move_base",true);
定义一个SimpleActionClient用于发送目标给move_base动作服务器。
//Wait 60 seconds for the action server to become available
if(!ac.waitForServer(ros::Duration(60)))
{
ROS_INFO("Can't connected to move base server");
return 1;
}
在开始发送目标之前，我们必须等到move_base动作服务器变成有效。这里是等待60秒。
//Cycle through the four waypoints
while( (count < 4) && (ros::ok()) )
{
//Update the marker display
marker_pub.publish(line_list);
//Intialize the waypoint goal
move_base_msgs::MoveBaseGoal goal;
//Use the map frame to define goal poses
goal.target_pose.header.frame_id = "map";
//Set the time stamp to "now"
goal.target_pose.header.stamp = ros::Time::now();
//Set the goal pose to the i-th waypoint
goal.target_pose.pose = pose_list[count];
//Start the robot moving toward the goal
//Send the goal pose to the MoveBaseAction server
ac.sendGoal(goal);
//Allow 1 minute to get there
bool finished_within_time = ac.waitForResult(ros::Duration(180));
//If we dont get there in time, abort the goal
if(!finished_within_time)
{
ac.cancelGoal();
ROS_INFO("Timed out achieving goal");
}
else
{
//We made it!
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Goal succeeded!");
}
else
{
ROS_INFO("The base failed for some reason");
}
}
count += 1;
}
首先我们发布标记表明四个目标姿势，每次循环都要执行，使标记贯穿整个移动过程。接着我们初始化MoveBaseGoal动作类型的goal变量。设置目标的frame_id为map坐标和时间戳为现在时间。设置目标姿势为现在的航点，并且发送目标给move_base动作服务器，等待180秒，返回true表明机器人在180秒内达到航点，返回flase表明没有达到航点。
仿真躲避障碍物的例子
1 启动roscore
roscore2 启动仿真机器人
roslaunch aicroboxi_bringup fake_aicroboxi.launch3 启动加载障碍物地图的map_server节点，启动move_base节点并加载全局和局部代价地图等配置文件
roslaunch robot_navigation fake_move_base_map_with_obstacles.launch
此.launch另外运行fake_move_base_obstacles.launch,
fake_move_base_obstacles.launch内容：
其中nav_obstacles_params.yaml内容如下：
-
TrajectoryPlannerROS: -
max_vel_x: 0.3 -
pdist_scale: 0.8 -
gdist_scale: 0.4
其实这些参数已经在base_local_planner_params.yaml文件用过，但设置这些参数为这些值为了更好地避开障碍物。此文件将最大速度从0.5m/s降到0.3m/s，也颠倒了pdist_scale和gdist_scale的相关权重，是因为更重视跟随计划的路径行走。本来也可以创建一个新的base_local_planner_params.yaml包括这些新的改变，但是通过这种方法我们可以在主要的文件保留大部分的参数，根据类似nav_obstacles_params.yaml这样特殊文件的需要来覆盖相应的参数。
4 启动 rviz 图形化显示程序
roslaunch robot_navigation view_nav_obstacles.launch
view_nav_obstacles.launch的内容如下：
5 启动move_base_square程序，效果如图3
rosrun robot_navigation move_base_square
在真正的机器人上测试避开障碍物
1 启动roscore
roscore2 启动机器人
roslaunch aicroboxi_bringup minimal.launch3 启动RGBD摄像头
roslaunch aicroboxi_bringup 3dsensor.launch4 启动带有地图的map_server节点，move_base节点，以及启动配置全局和局部代价地图
roslaunch robot_navigation tb_move_base_blank_map.launch
注意：此.launch文件不同于之前的仿真机器人的.launch文件。.yaml配置文件是参考《ROS by Example》关于TurtleBot的.yaml配置文件。在robot_navigation/config/turtlebot/目录下，也有四个.yaml配置文件。与《ROS by Example》的相比，主要修改一下几个参数：
-
base_local_planner_params.yaml： -
pdist_scale: 0.4 -
gdist_scale: 0.8
我自己经过多次测试，发现如果pdist_scale参数值比gdist_scale参数值大，即是更重视跟随规划路径行走，有时机器人会在障碍物旁边不断旋转，也总是尽可能贴近障碍物。为了避免刚才所说的情况，将pdist_scale参数值设置得比gdist_scale参数值小，即是更重视到达目的地，机器人能很顺利的通过障碍物并达到目的地。原因？有待探讨。
-
costmap_common_params.yaml: -
robot_radius: 0.17 -
inflation_radius: 0.3
将机器人的半径设置更大一些，机器人与障碍物之间距离设置大一些，目的为了防止笔记本电脑与机器人发生碰撞。
其他参数值请查看源代码。
5 启动 rviz 图形化显示程序
roslaunch robot_navigation view_nav_obstacles.launch6 启动move_base_square程序
rosrun robot_navigation move_base_square源代码地址：https://github.com/KeoChi/robot_navigation
参考地址：https://blog.csdn.net/u010925447/article/details/81294401