ROS 两轮差速自主导航小车记录（基础系列）

特别鸣谢：Unity-Drive Platform Group & Other group & Github_DLv
小车使用设备清单：rslidar_16线；xsens_imu；底层STM32F103读编码器；超声波；usb_cam
Authors：Kin_Zhang & All members in Unity-Drive

阅读前提：自己学习完 ROS_21讲

0 ROS_wiki
1 bilibli 古月居 ：av59458869
2 bilibli ROS机器人开发零基础入门 ：av50376766 & av50377985 -> 这个系列有两个
3 MOOC 机器人操作系统入门

其余ubuntu与插件便携系列在另一个博客里记录：ROS路程上的软件包及便携系列

多说一句:其实记录这个距离我知道真正学ROS前后没有1个月吧，做的基本都是实用性，过程告诉我，大家！有时间！一定要！多看源码！血泪教训啊，找一个对不上的 frame_id 找了一下午

• ✔0920更新：小张做完啦！成功跑完了navigation的所有操作，虽然都是用的包，但是用包的过程让我对ROS的整体框架也有了认识，（还有许多种error的情况啥的）;剩下就每天更新一下回忆一下整个过程和看一下概率机器人那本书（需要的话留下邮箱在评论区，我尽量都发到），原理得弄一弄；剩余8天回校
• ✔ 0927更新：今天基本把所有的东西做完了，把超声波转成laser信息加入障碍层(其实range_sonar_layer一直是一个范围层 从来不是障碍层！)这是看作者的issue和别人讨论的时候看到的，ps源码里也有 只对最大和最小进行判断
• ✔ 0928更新：今天试着把速度提上去了，发现Move_base里的最大速度竟然是由加速度限制的（详情见第七步）；走S形还是没有解决，但是速度上来了会有点弥补效果，后面有时间上传一下导航的效果视频。今天也算是走前更新完毕了，后面回校会尝试用一下gazebo继续小白的学习。（后面想做做视觉3维的导航）
• ✔ 0929更新：回校了，这贴算是更新全部完成了，看了一下该有的示意图都有了。
  小车3D图示意：（有意购买车身设备等请点击了解：Unity-Drive）
  

第一步：第一个node 第一个订阅

其实前人做了超多东西，底层32和ROS的通信串口什么的一应具全，全部写好了，由于某些原因这部分代码不能完全展示，但是大概就是打开它能看到以下信息：
主程序里需要订阅joy消息

/*Review */
ros::Subscriber joy_botton = nh.subscribe("/joy",10,JoyHandler);

之所以是本人的第一个Node是当时想用joy做一个按键切换自动导航还是手动驾驶。这个当时没想着记录大概点几个点：(这里的教训就是… 初次依赖那块和joy的消息格式不太清楚，特别是我一开始写的一直都是joystick->button(0)… 是后面才知道原来是这个格式
建议大家在学习过程中多多查看ROS_wiki和其包的源码

/*Review add joy.h */
#include  //务必！包含头文件啊
/*Review */
void JoyHandler(const sensor_msgs::JoyConstPtr &joystick )
{
    man_stop_button = joystick->buttons.at(2);
    auto_drive_button = joystick->buttons.at(0);
}

第二步：odom里程计信息

这里是拿嵌入式那边STM32串口读出来的p->data STM32发布的是/littlebot/encoder所以订阅的就是这个，稍后会添加图片给大家看一下消息内容示意：提示根据自己的消息类型查看

这个ROS_WIKI也有教程，成员也是根据odometry_tutorial的模式来写的
发布关于move_base支持的传感器的各种教程->Github链接

rostopic echo /littlebot/encoder

#include 
#include 
#include 
#include 
#include 
using namespace std;
#define PI 3.141592653
  double x = 0.0;
  double y = 0.0;
  double th = 0.0;  double vx = 0.0;
  double vy = 0.0;
  double vth = 0.0;  float dt = 0.05;
  double l = 0;
  double r = 0;
  double dl = 0;
  double dr = 0;
  
 void Encoder_Handler(const::std_msgs::Float32MultiArrayConstPtr p)   //订阅编码器的消息，转换为里程消息
{
            double left_pulse = p->data[1];
            double right_pulse = p->data[3];
      l = l + left_pulse;
      r = r + right_pulse;
      double dleft = left_pulse*PI*0.172/90; //计算左轮一周期内的运动路程，一圈为90个脉冲值
      double dright = right_pulse*PI*0.172/90; //计算右轮一周期内的运动路程
      dl = dl + dleft;
      dr = dr + dright;
      vx = (dleft+dright)/dt/2;
      vy = 0;
      vth = (dright-dleft)/dt/2/0.177;
      x += vx * cos(th+vth/2) * dt; 
      y += vx * sin(th+vth/2) * dt;
      th += vth * dt;
}
int main(int argc, char** argv)
{
	ros::init(argc, argv, "odometry_initial");  ros::NodeHandle n;
	ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("littlebot/odom", 50);
	ros::Subscriber encoder_sub = n.subscribe("/littlebot/encoder", 50, Encoder_Handler);  			
	ros::Time current_time, last_time;  ros::Rate r(10);
  	while(n.ok())
	  {
	    current_time = ros::Time::now();    ros::spinOnce();    //since all odometry is 6DOF we'll need a quaternion created from yaw
	    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);    //next, we'll publish the odometry message over ROS
	    nav_msgs::Odometry odom;
	    odom.header.stamp = current_time;
	    odom.header.frame_id = "odom";
	    odom.child_frame_id = "base_link";    //set the position
	    odom.pose.pose.position.x = x;
	    odom.pose.pose.position.y = y;
	    odom.pose.pose.position.z = 0.0;
	    odom.pose.pose.orientation = odom_quat;    //set the velocity
	    odom.twist.twist.linear.x = vx;
	    odom.twist.twist.linear.y = vy;
	    odom.twist.twist.angular.z = vth;
	    
	    // SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
	    odom.pose.covariance[0] = pow(10,-2); // = 0.01221 meters / sec
	    odom.pose.covariance[7] = pow(10,-2); // = 0.01221 meters / sec
	    odom.pose.covariance[14] = pow(10,6); // = 0.01221 meters / sec
	    
	    odom.pose.covariance[21] = pow(10,6); // = 0.41 degrees / sec
	    odom.pose.covariance[28] = pow(10,6); // = 0.41 degrees / sec
	    odom.pose.covariance[35] = pow(10,6);//0.2;// pow(0.1,2) = 0.41 degrees / sec    //publish the message
	    odom_pub.publish(odom);    last_time = current_time;
	    r.sleep();
	  }
	}
	

这样我们就得到了自己的odom信息，注意头文件的包含要在Package.xml和cmake文件中添加依赖等哦，不了解的请查看阅读前提处的课程

第三步：robot_pose_ekf

这个坑巨大！首先是！imu的frame_id一定得看清楚了，一定要出现红框的两个连接才是robot_pose_ekf起了作用就是数据链是连着的。
以下是配置的launch文件，

  <remap from="odom" to="/littlebot/odom" />
  <remap from="imu_data" to="/imu/data" />   

这里的remap就是把robot_pose_ekf的输入odom映射到/littlebot/odom上，也就是ekf融合的信息topic分别来自/littlebot/odom和/imu/data，也就是上图画出红圈圈的两个节点，（这里注意每个的frame_id一定要对应哦！imu在源码中是imu的id。错误信息：-> Could not transform imu message from %s to %s"

 <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  <param name="output_frame" value="odom"/>
  <param name="base_footprint_frame" value="base_link"/>
  <param name="freq" value="30.0"/>
  <param name="sensor_timeout" value="1.0"/>  
  <param name="odom_used" value="true"/>
  <param name="imu_used" value="true"/>
  <param name="vo_used" value="false"/>
  <param name="gps_used" value="false"/>
  <param name="debug" value="false"/>
  <param name="self_diagnose" value="false"/>
  <remap from="odom" to="/littlebot/odom" />
  <remap from="imu_data" to="/imu/data" />   
  
 node>

以上配置完成后launch一下，打开rqt_graph查看节点是否都已完成连接传输即可进行下一步。 （到这里完成的话 后面十分轻松，非常迅速）

第四步：tf_tree

如果上面的robot_pose_ekf没什么问题的话，后面就可以直接生成 -> tf 的变化图，本人最后不带map的图，生成这条线了才能 继续往下建图哦！特别是odom->base_link的

第五步：hector_mapping & karto_mapping

我也不知道gmapping为啥坑死在那个地方所以直接按着博客里的一个教程用的hector_mapping建图的，建图过程中请查看好自己的tf_tree

这个建图过程告诉我一件事就是：多了解某个领域的东西，多多尝试！ 走到hector_mapping的时候！想做个回环来闭环都不行，大概给大家看一下hector的建图效果，在我们办公室外的走廊走的一幅图… 左边是做了robot_pose_ekf的融合，右边直接拿的encoder去跑的（就是从一个起点跑过去 转一转然后原路跑回来，但是后面再大的时候效果就更差了）

效果更差图：

后续我大概贴一下launch文件，主要是注意激光雷达的frame_id变为laser或是mapping那边的scan信息 输入与激光雷达的输出对应，请经常用rqt_graph来检查node之间的连接，查看是否达到效果；
听师兄说，karto建图之所以更好是内部有激光雷达的闭环操作，具体操作呢… 我也没去看但是，确实建图效果十分ok，建图结果如下：

最重要karto超级容易设置，只要tf,scan信息都ok 记下就可以launch成功，… 我不知道为啥我的gmapping至今没解决那个问题
karto设置如下：

<node pkg="slam_karto" type="slam_karto" name="slam_karto" output="screen" launch-prefix="gnome-terminal -e">
  <remap from="scan" to="scan"/>
  <param name="odom_frame" value="odom"/>
  <param name="map_update_interval" value="25"/>
  <param name="resolution" value="0.025"/>
node>

第六步：amcl

这个没怎么设置，就跑通了，主要是发现amcl在运动过程中的定位效果通常更好。

<launch>
  
  <arg name="scan_topic"     default="scan"/>
  <arg name="initial_pose_x" default="0.0"/>
  <arg name="initial_pose_y" default="0.0"/>
  <arg name="initial_pose_a" default="0.0"/>
  
  <node pkg="amcl" type="amcl" name="amcl">    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="3000"/>
    <param name="kld_err"                   value="0.02"/>
    <param name="update_min_d"              value="0.20"/>
    <param name="update_min_a"              value="0.20"/>
    <param name="resample_interval"         value="1"/>
    <param name="transform_tolerance"       value="0.5"/>
    <param name="recovery_alpha_slow"       value="0.00"/>
    <param name="recovery_alpha_fast"       value="0.00"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <param name="gui_publish_rate"          value="50.0"/>    <remap from="scan"                      to="$(arg scan_topic)"/>
    <param name="laser_max_range"           value="3.5"/>
    <param name="laser_max_beams"           value="180"/>
    <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_likelihood_max_dist" value="2.0"/>
    <param name="laser_model_type"          value="likelihood_field"/>    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha1"               value="0.1"/>
    <param name="odom_alpha2"               value="0.1"/>
    <param name="odom_alpha3"               value="0.1"/>
    <param name="odom_alpha4"               value="0.1"/>
    <param name="odom_frame_id"             value="odom"/>
    <param name="base_frame_id"             value="base_footprint"/>  node>
launch>

整个amcl和move_base开启后的rqt_graph图：

第七步：navigation

主要使用的是move_base里的一系列的东西，
发现一个非常好的 解释move_base里一系列参数的东西的网址：

1. 在STDR中学习costmap代价地图
2. 在STDR中学习move_base及其导航系列

这里有个严重的问题就是小车的转向速度一直都有，但是明明global_plan路径规划的线很直，但是local_plan总是在转弯…（也不是什么大转弯 但是cmd_vel中有赋值，这个问题待解决，并没有看到有效方案）大概速度变换图我用PlotJuggler看了一下，如下：

右边两幅是路径我是走过去，又导航回来的路径，大致应该重合但是没有，所以amcl的定位效果还是很不错的。
左上角是速度变换图，linear是被我处理了 以免刹车太急，（取个平均处理法）

第二个问题是：最大速度由加速度决定（可能是速度切换太快所以加速度一下能加上去，修改地方为：dwa_local_planner_params.yaml

# The velocity when robot is moving in a straight line
  max_trans_vel:  1.5
  min_trans_vel:  0.5 #0.65
  max_rot_vel: 2
  min_rot_vel: 0.2
  
  acc_lim_x: 8.5 #4 !!这里 这个我的得加到8.5就能得到1.5m/s的速度
  acc_lim_y: 0.0 
  acc_lim_theta: 2.0

READ ME

/**
  ******************************************************************************
  * @file    READ ME
  * @author    Kin.Zhang 
  * @version V1.0.0
  * @date    2019-09-06
  ******************************************************************************
  * @attention
  *
  * Copyright (C) 2019 UDI Platform Group. All rights reserverd.
  ******************************************************************************
*/

#P.S.: already dialout chmod,source etc

#control
roslaunch ros_littlebot simple.launch
include: 
littlebot simple control & xsen_imu & tf->imu

#odom 
roslaunch lb_navigation lb_ekf_odom.launch
encoder odom & ekf_odom

#MAKE A MAP!
    #laser 激光雷达 & pointcloud_to_laser(for karto to make a map)
    roslaunch lb_navigation lb_kartomapping.launch
    #laser & pointcloud_to_laser(for hector to make a map)       
    roslaunch lb_navigation lb_hectormapping.launch
#NAVIGATION!
	#amcl location
	roslaunch lb_navigation amcl.launch
	#move_base
	roslaunch lb_navigation move_base.launch
	    
#ATTENTION:
save map:
cd kintest_ws/src/lb_navigation/maps/
rosrun map_server map_saver -f mapxxrecord bag:
cd ~
rosbag record -a

补充：将超声波加入costmap中

首先确认你的超声波的最小距离与最大距离，然后，根据navigation的tutorial教程将超声波转为laser信息，（我是这么干的… 可能有更好的方法，然后将我的代码贴在这里：）注意得根据自己的超声波来修改一些信息哦：，修改信息如下：

    scan.angle_min = -0.08;//!!!这里改超声波扫描角度范围
    scan.angle_max = 0.08;
    scan.range_min = 0.2; //!!!这里改最小距离
    scan.range_max = 4.0; //!!!这里改最大距离

总.cpp代码如下：

/**
  ******************************************************************************
  * @file    ultrasonic2laser
  * @author    Kin.Zhang 
  * @version V1.0.0
  * @date    2019-09-20
  ******************************************************************************
  * @attention
  *
  * Copyright (C) 2019 UDI Platform Group. All rights reserverd.
  ******************************************************************************
*/
#include 
#include 
#include 
double ul_Forward = 0.0;
double ul_Right = 0.0;
double ul_Left = 0.0;
double ul_Behind = 0.0;void ultrasonicCallback(const::std_msgs::UInt16MultiArrayConstPtr p)
{
  ul_Forward = p->data[0]/1000.0;
  ul_Right = p->data[1]/1000.0;
  ul_Left = p->data[3]/1000.0;
  ul_Behind = p->data[2]/1000.0;
}
sensor_msgs::LaserScan pub_laser(double ultrasonic, int Dir)
{
  sensor_msgs::LaserScan scan;
  unsigned int num_readings = 300;
  double laser_frequency = 100;
  double ranges[num_readings];
  double intensities[num_readings];  //generate some fake data for our laser scan
  for(unsigned int i = 0; i < num_readings; ++i)
  {
      ranges[i] = ultrasonic;
      intensities[i] = 0;
  }
    ros::Time scan_time = ros::Time::now();    //populate the LaserScan message
    
    scan.header.stamp = scan_time;    if (Dir == 1)     scan.header.frame_id = "/ultrasonic_Forward";
    else if(Dir == 2) scan.header.frame_id = "/ultrasonic_Right";
    else if(Dir == 3) scan.header.frame_id = "/ultrasonic_Left";
    
    scan.angle_min = -0.08;//!!!这里改超声波扫描角度范围
    scan.angle_max = 0.08;
    scan.angle_increment = (scan.angle_max-scan.angle_min) *1.0 / num_readings;
    scan.time_increment = (1 / laser_frequency) / (num_readings);
    scan.range_min = 0.2; //!!!这里改最小距离
    scan.scan_time = (1 / laser_frequency);
    scan.range_max = 4.0; //!!!这里改最大距离
    scan.ranges.resize(num_readings);
    for(unsigned int i = 0; i < num_readings; ++i)
    {
      scan.ranges[i] = ranges[i];
    }
    return scan;}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "ultrasonic2laser");  ros::NodeHandle n,n2;
  ros::Publisher scan_pubF = n.advertise<sensor_msgs::LaserScan>("ultrasonic_laserF", 50);
  ros::Publisher scan_pubR = n.advertise<sensor_msgs::LaserScan>("ultrasonic_laserR", 50);
  ros::Publisher scan_pubL = n.advertise<sensor_msgs::LaserScan>("ultrasonic_laserL", 50);
  ros::Subscriber ultrasonic_sub=n2.subscribe("/littlebot/ultrasonic", 50, ultrasonicCallback);
  ros::Rate r(1.0);
  while(n.ok())
  {
    ros::spinOnce();               // check for incoming messages
    
    scan_pubF.publish(pub_laser(ul_Forward,1));
    scan_pubR.publish(pub_laser(ul_Right,2));
    scan_pubL.publish(pub_laser(ul_Left,3));
    
    r.sleep();
  }
}

插入加入超声波的显示图：
灰色是laser信息，彩色是costmap信息

但是由于我自己的超声波是串口输出有时候传输速度跟不上没办法刷新的很快，这个超声波转为laser后可以弥补激光雷达在玻璃处丢失定位的一些弊端，大家可以尝试把120°夹角的三个超声波转为一个激光雷达的360°，这样能和激光雷达共同工作。

以上，前人做的功课很多，才让小张能快速上手，但愿把这段时间的一些问题和找bug的都记录了吧。欢迎大家有问题多交流