自制基于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点击地图即可为机器人设置导航目标点，机器人会自动规划出行进路线，机器人会循着路径到达设置的地点。