实时建立环境地图并定位，要依靠于昂贵的激光雷达，不过现在有了可替代的工具，可以伪造出激光雷达的效果，算是伪造吧。
可替代的工具是微软的Kinect和Asus Xtion相机。可以看depthimage_to_laserscan和kinect_2d_scanner 两个包的内容。
这一章主要以3个包的内容展开航行的设计：
- move_base：在给定的参考系内移动机器人到指定点
- gmapping：由激光雷达数据生成地图（或者深度相机数据）
- amcl：在已有的地图内定位机器人
完成了上面这些之后，我们就可以实现命令机器人去地图内的任意位置并且它会自动避障。关于SLAM理论，可以查看此文章：Artificial Intelligence
1 使用move_base规划路径及避障
1.1 使用move_base指定航行目标
move_base使用 MoveBaseActionGoal message ，看一下消息的定义：
rosmsg show MoveBaseActionGoal
会显示以下信息：
[move_base_msgs/MoveBaseActionGoal]:
std_msgs/Header header
uint32 seq
time stamp
string frame_id
actionlib_msgs/GoalID goal_id
time stamp
string id
move_base_msgs/MoveBaseGoal goal
geometry_msgs/PoseStamped target_pose
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
这个看起来有点复杂，我们下面会使用简单一点的东西来指定目标。
1.2 为路径规划配置参数
move_base运行之前需要设置四个文件。这些文件定义了障碍，机器人半径，路径需要规划多远，机器人运行多快等。
这四个设置文件可以在config文件夹下找到：
- base_local_planner_params.yaml
- costmap_common_params.yaml
- global_costmap_params.yaml
- local_costmap_params.yaml
1.3 仿真测试move_base
move base节点需要一个环境地图才能运行，不过使用一个空地图也是可以的。我们后面会使用真正的地图。rbx1_navpackage包含了一个空地图叫做blank_map.pgm，它在maps的子目录下。描述文件叫blank_map.yaml。启动move_base节点和空地图的启动文件叫fake_move_base_blank_map.launch，它在launch的子目录下。
现在来看一下启动文件。
<launch>
<!-- Run the map server with a blank map -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
<include file="$(find rbx1_nav)/launch/fake_move_base.launch" />
<!-- Run a static transform between /odom and /map -->
<node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
</launch>
首先在一个空白地图上启动了mao_server node.地图的描述文件就是那个.yaml文件.
然后加载了fake_move_base.launch文件,它启动了move_base node并且加载了必要的参数.
最后,因为我们使用了空白的地图并且我们的仿真机器人没有传感器,机器人不能使用扫描数据定位.我们对机器人量程框架和地图框架,或者说坐标系,做一个静态的简单对应,换句话说,就是假设机器人的编码器能够获得理想的数据.
然后我们再看一下fake_move_base.launch文件:
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />
<rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />
<rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />
</node>
</launch>
这个启动文件运行了move_base node和五个rosparam来导入参数.costmap_common_params.yaml导入了两次,是为了把这些参数同时设置在global_costmap namespace 和 local_costmap namespace.使用后面的ns来指定.
要在仿真中尝试,首先启动ArbotiX仿真器:
roslaunch rbx1_bringup fake_turtlebot.launch
这里可以换成别的机器人.
然后在空白地图上启动move_base node:
roslaunch rbx1_nav fake_move_base_blank_map.launch
如果你还没有运行过RViz,可以使用配置好的参数文件启动:
rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz
现在我们已经准备好了使用move_base控制机器人,而不是简单的使用Twist消息.
为了测试一下.首先我们让机器人前进1米.现在我们的机器人位于(0,0,0)在/map坐标系和/base_link坐标系.我们可以使用任意一个坐标系指定这次移动.
然而,第一次移动并不能让机器人到达准确的位置,随后的误差要靠/base_link坐标系去比较消除.所以我们最好在/map上面设定目标.指令如下:
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
'{ header: { frame_id: "map" }, pose: { position: { x: 1.0, y: 0, z:
0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
把机器人移动回原点,只要停止刚才的命令,然后按照相同的格式输入原点坐标就可以了,像下面这样:
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
'{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z:
0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
你可以看到一个细细的绿线,那个是全局路径规划,还可以看到一个红线,是实时更新的本地路径规划.想要更为清晰的看到这两条线,可以在RViz上面关掉Odometry, Goal
Pose and Mouse Pose,然后重新运行上面的命令.
绿色的路径比较平坦,是因为这中间没有任何的障碍,另外,它还跟我们的一些参数设置有关.比如,pdist_scale (0.4) and gdist_scale (0.8),还有最大线速度( max_vel_x ).我们的局部路径,跟我们的全局规划路径相差很大,想要让我们的机器人更加贴近我们的全局规划路径,我们可以使用rqt_reconfigure增大pdist_scale参数或者减小max_vel_x.
再打开一个新窗口,启动rqt_reconfigure:
rosrun rqt_reconfigure rqt_reconfigure
然后,打开move_base->TrajectoryPlannerROS,把pdist_scale设置的大一点,比如0.8,然后把gdist_scale设置的小一点,比如0.4.然后重新运行运动指令,看看有什么变化.
好像是好了很多.
pdist_scale: 0.8//控制器距离给定的路径有多近的加权值
gdist_scale: 0.4////控制器试图达到局部目标，或者是控制速度的加权值
1.4 使用点击指定目标
我们刚刚是使用nav.rviz文件启动RViz的,这样我们可以直接点击2D Nav Goal在地图上指定目标.点击时不要放开,可以旋转改变设定目标的方向.
我们可以再RViz的窗口上看到设置信息:
1.5 使用move_base导航一个正方形
我们最好重新开始,关掉之前所有的node,然后:
roslaunch rbx1_bringup fake_turtlebot.launch
roslaunch rbx1_nav fake_move_base_blank_map.launch
rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz
然后执行命令:
rosrun rbx1_nav move_base_square.py
程序里面有很多注释,可以自己打开看一下.
1.6 避障
move_base最厉害的一点是,它可以在到达指定位置的同时躲避障碍.局部路径规划会重新计算路径.
我们将会打开一个带有障碍的地图,然后仍然使用move_base_square.py运行机器人,看它是否会避开障碍,并且到达目标.
首先打开地图:
roslaunch rbx1_bringup fake_turtlebot.launch
然后清理到move_base节点的资源:
rosparam delete /move_base
这个命令会清理掉所有move_base已经退出的参数,它的清理程度仅次于重启roscore.
然后运行加载地图和move_base:
roslaunch rbx1_nav fake_move_base_map_with_obstacles.launch
然后运行RViz:
rosrun rviz rviz -d `rospack find rbx1_nav`/nav_obstacles.rviz
黄色的部分就是障碍,其他颜色代表一个扩大的安全距离的缓冲.
也可以使用点击,像之前那样,设定目标.
