自制基于ROS与Arduino小车导航（）

本制作采用了：上位机----tx2（ubuntu16.04）
下位机----Arduino 2560
传感器-----思岚rplidar A2

代码见:https://github.com/crjxixixi/cleaner_robot1.git

现在主要对自己的上位机代码进行一些解释：

一、建图

建立地图的方式有两种：gmapping算法与 hector算法
区别：Gmapping 算法在建立地图时需要机器人雷达的扫描信息与里程计信息
Hector 算法只需要机器人的雷达扫描信息
本实验采取的是hector进行建图：

（1）准备工作：安装hector_slam库

sudo apt-get install ros-kinetic-hector-slam

（2） 创建 hector.launch（本代码在/src/mrobot_navigation/launch）

<launch>


<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />


<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>


<!-- Map size / start point -->
<param name="map_resolution" value="0.05"/>
<param name="map_size" value="512"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="laser_z_min_value" value = "-1.0" />
<param name="laser_z_max_value" value = "1.0" />
<param name="map_multi_res_levels" value="2" />


<param name="map_pub_period" value="2" />
<param name="laser_min_dist" value="0.4" />
<param name="laser_max_dist" value="5.5" />
<param name="output_timing" value="false" />
<param name="pub_map_scanmatch_transform" value="true" />
<!--<param name="tf_map_scanmatch_transform_frame_name" value="scanmatcher_frame" />-->


<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.7" />    
<param name="map_update_distance_thresh" value="0.2"/>
<param name="map_update_angle_thresh" value="0.06" />


<!-- Advertising config --> 
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="5"/>
<param name="scan_topic" value="scan"/>
</node>


<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 /base_link /laser 100"/>


  <node pkg="rviz" type="rviz" name="rviz"
    args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>


</launch>

若遇到有不能启动则根据提示安装相应的包就可运行，随着机器人的移动可以在rviz可视化界面中看到你的栅格地图构建的情况。

（3）地图保存

在地图保存之前，需要安装map_server包，在终端中输入：

sudo apt-get install ros-kinetic-map-server

然后，保存图形：

rosrun map_server map_saver -f ~/my_map

～处可以修改为你要存放的地址，本操作将地址保存在根目录下。

二、导航

由于每个人的机器人外形都不相同，大家可以自己去编写适合自己机器人的urdf，大家可以参考 urdf古月居

在本实验中，由于下位机是采取的Arduino,则在开发时可以将Arduino定义为一个结点来收发话题，从而方便开发（若使用的是stm32进行开发的则需要自己编写串口通信）
1.准备工作
(1)若想要学习如何将Arduino定义为一个结点的大家可以到创客智造中找到相应的教程:
https://www.ncnynl.com/archives/201701/1215.html
(2)Arduino IDE的安装
可以参考教程：https://jingyan.baidu.com/article/219f4bf7e45df3de442d388e.html
(3)ROS相应的导航包安装

2.编写相应的launch文件(可见代码/src/mrobot_navigation/mrobot_nav.launch)

<launch>
    <param name="use_sim_time" value="false" />
    <arg name="use_map_topic" default="false"/>
	 <arg name="scan_topic" default="scan"/>

    <!-- 配置雷达 -->
    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
       <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
       <param name="serial_baudrate"     type="int"    value="115200"/>
       <param name="frame_id"            type="string" value="laser"/>
       <param name="inverted"            type="bool"   value="false"/>
       <param name="angle_compensate"    type="bool"   value="true"/>
    </node>  
    <!--地图信息-->
    <arg name="map_file" default="/home/xt/my_map.yaml"/>
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
    <!--机器人 move_base -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="false">
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/base_local_planner_params.yaml" command="load" />
    </node>
 <include file="$(find mrobot_navigation)/launch/amcl.launch" /> 
   <!-- 设置坐标-->
   <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" /> 

   <!-- 运行rviz -->
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/nav.rviz"/>
</launch>

根据自己的传感器进行相应的配置，本实验中用的是思岚的rplidar A2雷达，所以该launch文件首先对雷达进行了配置

< laser >

    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
       <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
       <param name="serial_baudrate"     type="int"    value="115200"/>
       <param name="frame_id"            type="string" value="laser"/>
       <param name="inverted"            type="bool"   value="false"/>
       <param name="angle_compensate"    type="bool"   value="true"/>

设置雷达的串口号、波特率等一些参数，在使用雷达的时候需要给串口权限（根据自己的串口号），本实验中用的是ttyUSB0则给权限如下：

sudo chmod 777 /dev/ttyUSB0

接着加载你刚建好的地图信息：
< map >

    <arg name="map_file" default="/home/xt/my_map.yaml"/>
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

将default后面的参数为你地图所在的路径

< move_base > 主要配置机器人的移动时的一些参数（如速度、避障、地图膨胀、路径规划算法）等

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="false">
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find mrobot_navigation)/config/mrobot1/base_local_planner_params.yaml" command="load" />
    </node>
 </node>  

其中base_local_planner_params.yaml、costmap_common_params.yaml、global_costmap_params.yaml、local_costmap_params.yaml文件各参数的配置及其各参数的含义可以看此教程https://www.guyuehome.com/28164

< amcl >

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- translation std dev, m -->

<param name="global_frame_id" value="/map" />
<param name="initial_pose_x" value="0.0"/> 
    <param name="initial_pose_y" value="0.0"/> 
    <param name="initial_pose_a" value="0.0"/> 
    <param name="initial_cov_xx" value="0.5*0.5"/> 
    <param name="initial_cov_yy" value="0.5*0.5"/>
    <param name="initial_cov_aa" value="(π/12)*(π/12)"/> 

        <param name="odom_alpha3" value="0.2"/>
        <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_model_type" value="likelihood_field"/>
  <param name="initial_pose_x" value="0.0"/> 
   <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_max_range" value="12.0"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <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"/>
       
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.25"/>
        <param name="update_min_a" value="0.2"/>
       <param name="odom_frame_id" value="odom"/>
	<param name="base_frame_id" value="base_link"/> 
        <param name="resample_interval" value="1"/>
        <!-- Increase tolerance because the computer can get quite busy -->
        <param name="transform_tolerance" value="1.0"/>
        <param name="recovery_alpha_slow" value="1.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>

此结点是用于发布机器人的定位信息的功能，详细情况可以参考：http://wiki.ros.org/amcl，大致来说，amcl需要订阅机器人的消息有 1.传感器的信息 2.tf变换 3.地图信息 4.机器人的初始位置，从而通过这些信息来发布 1.机器人的位姿 2.粒子云 3./tf变换(/map–>/odom)，这样机器人就能实时的掌握自己所处的位置，并通过mover_base来进行导航。

< rviz >

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/nav.rviz"/>

打开rviz可视化工具可以看到机器人在地图上的位置，与导航算法所规划出来的路径。

2.导航操作
导航前需要使用2D Post estimate对机器人位姿进行初始化，使得机器人雷达扫描点与之前所建地图尽可能重合。初始点的好坏对机器人导航效果有着一定的影响。
使用2D NAV Goal点击地图即可为机器人设置导航目标点，机器人会自动规划出行进路线，机器人会循着路径到达设置的地点。