本节目标:加载一张地图,在地图中放置一个机器人实现对机器人的定位。
实现:
- 打开gazebo和模型文件(car_move.launch)
- 加载地图
- 启动机器人(gazebo)
- 启动rviz,完成rviz配置
前期准备
- 延用工作空间test2_ws;
- 功能包demo5_navigation存放代码;
- amcl和rviz代码在demo5_navigation/launch/nav2_test.launch;
- amcl代码demo5_navigation/launch/nav2_amcl.launch;
- rviz配置在demo5_navigation/config/rviz/nav2.rviz;
简单介绍
SLAM中的定位是指全局定位用于地图。这里的定位是指当前定位,用于导航。在ROS的导航功能包集中的AMCL就是一种提供给机器人定位的功能包。被集成到了navigation中。核心节点AMCL。在amcl功能包下,有差分diff和全向移动omni的例子,本节选用差速为例修改:
roscd amcl
cd examples
gedit amcl_diff.launch
I、 ACML文件
对照ACML的例子编写launch/nav2_amcl.launch,主要修改三个坐标系:
<param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
<param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
<param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->
II、汇总launch文件
launch/nav2_test.launch
实现打开rviz,加载地图,执行acml的过程。
III、执行查看
- 加载机器人模型(gazebo)
roslaunch demo4_urdf_gazebo car_move.launch
- 执行AMCL算法
roslaunch demo5_navigation/launch/nav2_test.launch
- 配置rviz并保存,本节重要的主要是pasearray
- 键入控制机器人运动:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
可以看到线束最多的地方就是概率最大的位置。
源码
nav2_amcl.launch
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<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_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
<param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
<param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->
<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>
nav2_test.launch
这里已经保存了rviz的配置为nav2.rviz并读取。
<launch>
<!-- 启动 rviz -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find demo5_navigation)/config/rviz/nav2.rviz" />
<!-- 加载地图服务 -->
<include file="$(find demo5_navigation)/launch/nav1_pub_map.launch" />
<!-- AMCL文件 -->
<include file="$(find demo5_navigation)/launch/nav2_amcl.launch" />
</launch>