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 <sensor_msgs/Joy.h> //務必！包含頭文件啊
/*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 <iostream>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float32MultiArray.h>
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" />   
  <!--_data _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>
  <!-- Arguments -->
  <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"/>
  <!-- AMCL -->
  <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 <[email protected]>
  * @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 <[email protected]>
  * @version V1.0.0
  * @date    2019-09-20
  ******************************************************************************
  * @attention
  *
  * Copyright (C) 2019 UDI Platform Group. All rights reserverd.
  ******************************************************************************
*/
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/UInt16MultiArray.h>
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的都記錄了吧。歡迎大家有問題多交流