ROS机器人仿真（六）- 航行，路径规划和SLAM

实时建立环境地图并定位，要依靠于昂贵的激光雷达，不过现在有了可替代的工具，可以伪造出激光雷达的效果，算是伪造吧。
可替代的工具是微软的Kinect和Asus Xtion相机。可以看depthimage_to_laserscan和kinect_2d_scanner 两个包的内容。
这一章主要以3个包的内容展开航行的设计：

move_base使用 MoveBaseActionGoal message ，看一下消息的定义：
rosmsg show MoveBaseActionGoal
会显示以下信息：

这个看起来有点复杂，我们下面会使用简单一点的东西来指定目标。

move_base运行之前需要设置四个文件。这些文件定义了障碍，机器人半径，路径需要规划多远，机器人运行多快等。
这四个设置文件可以在config文件夹下找到：

move base节点需要一个环境地图才能运行，不过使用一个空地图也是可以的。我们后面会使用真正的地图。 rbx1_nav package包含了一个空地图叫做 blank_mappgm ，它在maps的子目录下。描述文件叫blank_mapyaml。启动move_base节点和空地图的启动文件叫fake_move_base_blank_maplaunch，它在launch的子目录下。
现在来看一下启动文件。

首先在一个空白地图上启动了mao_server node地图的描述文件就是那个yaml文件
然后加载了fake_move_baselaunch文件,它启动了move_base node并且加载了必要的参数
最后,因为我们使用了空白的地图并且我们的仿真机器人没有传感器,机器人不能使用扫描数据定位我们对机器人量程框架和地图框架,或者说坐标系,做一个静态的简单对应,换句话说,就是假设机器人的编码器能够获得理想的数据
然后我们再看一下fake_move_baselaunch文件:

这个启动文件运行了move_base node和五个rosparam来导入参数costmap_common_paramsyaml导入了两次,是为了把这些参数同时设置在global_costmap namespace 和 local_costmap namespace使用后面的ns来指定
要在仿真中尝试,首先启动ArbotiX仿真器:

这里可以换成别的机器人
然后在空白地图上启动move_base node:

如果你还没有运行过RViz,可以使用配置好的参数文件启动:

现在我们已经准备好了使用move_base控制机器人,而不是简单的使用Twist消息
为了测试一下首先我们让机器人前进1米现在我们的机器人位于(0,0,0)在/map坐标系和/base_link坐标系我们可以使用任意一个坐标系指定这次移动
然而,第一次移动并不能让机器人到达准确的位置,随后的误差要靠/base_link坐标系去比较消除所以我们最好在/map上面设定目标指令如下:

把机器人移动回原点,只要停止刚才的命令,然后按照相同的格式输入原点坐标就可以了,像下面这样:

你可以看到一个细细的绿线,那个是全局路径规划,还可以看到一个红线,是实时更新的本地路径规划想要更为清晰的看到这两条线,可以在RViz上面关掉Odometry, Goal
Pose and Mouse Pose,然后重新运行上面的命令

绿色的路径比较平坦,是因为这中间没有任何的障碍,另外,它还跟我们的一些参数设置有关比如,pdist_scale (04) and gdist_scale (08),还有最大线速度( max_vel_x )我们的局部路径,跟我们的全局规划路径相差很大,想要让我们的机器人更加贴近我们的全局规划路径,我们可以使用rqt_reconfigure增大pdist_scale参数或者减小max_vel_x
再打开一个新窗口,启动rqt_reconfigure:

然后,打开move_base->TrajectoryPlannerROS,把pdist_scale设置的大一点,比如08,然后把gdist_scale设置的小一点,比如04然后重新运行运动指令,看看有什么变化
好像是好了很多

我们刚刚是使用navrviz文件启动RViz的,这样我们可以直接点击2D Nav Goal在地图上指定目标点击时不要放开,可以旋转改变设定目标的方向
我们可以再RViz的窗口上看到设置信息:

我们最好重新开始,关掉之前所有的node,然后:

然后执行命令:

程序里面有很多注释,可以自己打开看一下

move_base最厉害的一点是,它可以在到达指定位置的同时躲避障碍局部路径规划会重新计算路径
我们将会打开一个带有障碍的地图,然后仍然使用move_base_squarepy运行机器人,看它是否会避开障碍,并且到达目标
首先打开地图:

然后清理到move_base节点的资源:

这个命令会清理掉所有move_base已经退出的参数,它的清理程度仅次于重启roscore
然后运行加载地图和move_base:

然后运行RViz:

的部分就是障碍,其他颜色代表一个扩大的安全距离的缓冲
也可以使用点击,像之前那样,设定目标

        使用笔记本自带摄像头跑slam，在此之前需要标定笔记本相机。因为使用的是虚拟机下ubuntu14系统，在标定相机时遇到了网上标定教程没有出现的问题。标定前需要安装ros相关软件包。

  (1)   安装相关软件包：

         sudo apt-get install ros-indigo-usb-cam    // 用于接收和发布摄像头数据topic

         sudo apt-get install ros-indigo-image-view // 用于显示

         sudo apt-get install ros-indigo-camera-calibration //用于相机标定

  (2)  发布image topic：  

        相机标定首先要使用usb-cam来获取摄像头数据，运行usb-cam launch文件

         roslaunch usb_cam usb_cam-testlaunch 

          如果成功，则会d出摄像头捕捉的视频窗口，同时rostopic list 出现 /image_raw 和 /camera_info两个话题。但是这一步对于使用虚拟机的同学会遇到很多问题，不过按照如下步骤应该没有问题：

          1）确认camera状态和信息（使用v4lz来 *** 作，这玩意专门用来 *** 作摄像头）：

                v4l2-ctl --list-devices   //查询摄像头设备，如输出： /dev/video0  输出设备编号，这个待会用来核对launch文件

               如果没有输出设备编号，说明虚拟机根本没有连接上摄像头，可以通过虚拟机菜单"虚拟机—可移动设备—xx camera—连接"，然后再运行此命令，应该就有编号了，说明链接成功，然后按照编号修改对应的launch文件，一般都是video0，不用修改。

            2）v4l2-ctl --list-formats-ext // 查询视频流格式吧？我猜的，不过这个很重要，会输出当前摄像头支持的视频流格式、帧率、图像尺寸等信息，用来修改launch文件

            经过查询发现，虚拟机摄像头支持yuyv和mjpeg两种视频格式，而launch文件默认选择的是yuyv格式，实际使用发现，虚拟机下是不支持这种格式的，所以替换成mjepg格式，其他尺寸可以根据上述命令的输出自己选择。

            3）出现视频窗口，有图像后突然卡死时，更改虚拟机usb设置，将usb兼容调整为11，或者别的，我的是11，重新启动后不会卡死。

    (3) 运行标定程序：

         在此之前你需要准备棋盘，如下，打印尺寸可以自己选择，然后运行：

         rosrun camera_calibration cameracalibratorpy --size 8x6 --square 0025 image:=/usb_cam/image_raw camera:=/usb_cam // 其中 size是黑白格的横纵点数，square是黑白格边长，image是图像节点名称，camera是相机名称

    (4) 开始标定：

      参照标定说明

---