1. 爲機器人配置TF

1.1 變換配置

許多ROS軟件包都要使用TF軟件功能包，以機器人識別的變換樹的形式發佈。抽象層面上，變換其實就是一種“偏移”（包括平移和旋轉），代表了不同座標系之間的變換和旋轉。

更具體點來說，設想一個簡單的機器人，只有一個基本的移動機體和掛在機體上方的掃描儀。基於此，我們定義了兩個座標系：一個對應於機體中心點的座標系，一個對應於掃描儀中心的座標系。分別取名爲“base_link”和“baser_laser。

此時，可以假設，我們已經從傳感器獲取了一些數據，以一種代表了物體到掃描儀中心點的距離的形式給出。換句話說，我們已經有了一些“base_laser”座標系的數據。現在，我們期望通過這些數據，來幫助機體避開物理世界的障礙物。成功的關鍵是，我們需要一種方式，把傳感器掃描的數據，從“base_laser”座標系轉換到“base_link”座標系中去。本質上，就是定義一種兩個座標系的“關係”。

爲了定義這種關係，假設我們知道，傳感器是掛在機體中心的前方10cm，高度20cm處。這就等於給了我們一種轉換的偏移關係。具體來說，就是，從傳感器到機體的座標轉換關係應該爲(x:0.1m,y:0.0m, z:0.2m),相反的轉換即是(x:-0.1m,y:0.0m,z:0.2m)。

可以理解成傳感器到機體中心的向量（0.1,0.0,0.2）

可以自己去定義變換關係，以及完成變換的代碼。但是隨着座標系的增多，每對座標系之間如果都有相應的變換關係時，就會顯得複雜，而且沒有標準。

tf來管理這種關係，我們需要把他們添加到轉換樹（transform tree）中。一方面來說，轉換樹中的每一個節點都對應着一類座標系，節點之間的連線即是兩個座標相互轉換的一種表示，一種從當前節點到子節點的轉換表示。Tf利用樹結構的方式，保證了兩個座標系之間的只存在單一的轉換，同時假設節點之間的連線指向是從parent到child。

基於我們簡單的例子，我們需要創建兩個節點，一個“base_link”，一個是“base_laser”。爲了定義兩者的關係，首先，我們需要決定誰是parent，誰是child。時刻記得，由於tf假設所有的轉換都是從parent到child的，因此誰是parent是有差別的。我們選擇“base_link”座標系作爲parent，其他的傳感器等，都是作爲“器件”被添加進robot的，對於“base_link”和“base_laser”他們來說，是最適合的。這就意味着轉換關係的表達式應該是(x:0.1m,y0.0m,z:0.2m)。關係的建立，在收到“base_laser”的數據到“base_link”的轉換過程，就可以是簡單的調用tf庫即可完成。我們的機器人，就可以利用這些信息，在“base_link”座標系中，就可以推理出傳感器掃描出的數據，並可安全的規劃路徑和避障等工作。

假定，我們以上層來描述“base_laser”座標系的點，來轉換到”base_link”座標系。首先，我們需要創建節點，來發布轉換關係到ROS系統中。下一步，我們必須創建一個節點，來監聽需要轉換的數據，同時獲取並轉換。在某個目錄創建一個源碼包，同時命名“robot_setup_tf”。添加依賴包roscpp,tf,geometry_msgs。

catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

廣播變換

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;//使用它來完成廣播

  while(n.ok()){
  //發送tf函數，有5個參數
    broadcaster.sendTransform(
    //arg1 變換關係（父到子），它也有2個參數，旋轉和位移
      tf::StampedTransform(
            tf::Quaternion(0, 0, 0, 1), 
            tf::Vector3(0.1, 0.0, 0.2)
        ),
    //發送時間
        ros::Time::now(),
    //父節點名
        "base_link", 
    //子節點名
        "base_laser")
        );
    r.sleep();
  }
}

調用變換

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

/*創建一個函數，參數爲TransformListener,作用爲將“base_laser”座標系的點，變換到“base_link”座標系中。這個函數將會以ros::Timer定義的週期，作爲一個回調函數週期調用。目前週期是1s*/

void transformPoint(const tf::TransformListener& listener)
{
/*創建一個虛擬點，作爲geometry_msgs::PointStamped。消息名字最後的"Stamped"的意義是，它包含了一個頭部，允許我們去把時間戳和消息的frame_id相關關聯起來。*/
  geometry_msgs::PointStamped laser_point;
/*關聯frame_id也就是將node定義爲frame_id*/
  laser_point.header.frame_id = "base_laser";
/*設置laser_point的時間戳爲ros::time(),即是允許我們請求TransformListener取得最新的變換數據*/
  laser_point.header.stamp = ros::Time();
//創建虛擬的點，即要變換過去的數據  
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
/*通過TransformListener對象，調用transformPoint(),填充三個參數來進行數據變換*/
/*arg1，代表我們想要變換的目標座標系的名字。
  arg2，填充需要變換的原始座標系的點對象。
  arg3，目標座標系的點對象。
  所以，在函數調用後，base_point裏存儲的信息就是變換後的點座標。*/
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

/*某些其他的原因，變換不可得（可能是tf_broadcaster 掛了），調用transformPoint()時，TransformListener調用可能會返回異常。爲了體現代碼的優雅性，我們將會截獲異常並把異常信息呈現給用戶。*/
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;
//自動的訂閱ROS系統中的變換消息主題，同時管理所有的該通道上的變換數據
  tf::TransformListener listener(ros::Duration(10));


  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

修改CMakeList.txt

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
target_link_libraries(tf_listener ${catkin_LIBRARIES})

編譯並運行

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19
[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19
[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19
[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19
[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

可以看到兩個座標系點的位置。

1.2發佈里程信息

一個通過ROS從/odom座標系到/base_link座標系的變換信息，包括通過ROS發送nav_msgs/Odometry消息。

導航功能包集用TF軟件包來確定機器人在世界座標系中的位置和相對於靜態地圖的相關傳感器信息，但是TF軟件包不提供與機器人速度相關的任何信息，所以導航功能包集要求里程計源程序發佈一個變換和一個包含速度信息的nav_msgs/Odometry消息。

##nav_msgs/Odometry消息
std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
      geometry_msgs/Pose pose
            geometry_msgs/Point position
                  float64 x
                  float64 y
                  float64 z
            geometry_msgs/Quaternion orientation
                  float64 x
                  float64 y
                  float64 z
                  float64 w
      float64[36] covariance
geometry_msgs/TwistWithCovariance twist
      geometry_msgs/Twist twist
            geometry_msgs/Vector3 linear
                  float64 x
                  float64 y
                  float64 z
            geometry_msgs/Vector3 angular
                  float64 x
                  float64 y
                  float64 z
      float64[36] covariance

nav_msgs/Odometry消息存儲機器人在自由空間位置和速度的估計。在這個消息中，pose與機器人相對於里程計座標系的爲hi在估計與位置估計的協方差相關，twist與在子座標系裏機器人的速度相關，通常是移動平臺的座標系連同速度估計的協方差。

使用tf發佈里程計變換

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
//發佈odom座標系到base_link座標系的變換和nav_msgs/Odometry消息
//需要的頭文件
int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
//創建publisher和broadcaster實例，以便能夠分別使用ROS和tf發送消息
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;
//假設機器人最初從odom座標系的原點開始
  double x = 0.0;
  double y = 0.0;
  double th = 0.0;
//設置一些速度，讓base_link座標系相對於odom座標系 在x上0.1m/s 
//y上-0.1m/s的速率 th方向0.1rad/s角度移動，即轉圈
  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();
//以1hz速率發佈odom里程計的信息
  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages,書上沒有這一句
    current_time = ros::Time::now();


//根據設置恆定的速度更新里程信息
//真正的里程系統將整合計算的速度
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

//通常嘗試使用我們系統中所有消息的3D版本，以允許2D和3D組件在適當的時候一起工作，並將我們要創建的消息數量保持在最小。
//因此，有必要將我們用於里程的偏航值轉換爲四元數發送出去
//幸運的是，tf提供了允許從偏航值容易地創建四元數並且容易地訪問四元數的偏航值的功能。

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

//創建一個TransformStamped消息，將通過tf發送
//在current_time發佈從odom座標系到base_link座標系的轉換
//確保使用odom爲父座標系 base_link爲子座標系

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

//從我們的odometry數據填充變換消息，然後使用TransformBroadcaster發送變換。
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;


    odom_broadcaster.sendTransform(odom_trans);

//還需要發送nav_msgs/Odometry消息，以便導航堆棧可以從中獲取速度信息。
//將消息的頭部設置爲current_time和odom座標系

    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

//使用里程數據填充消息，並且通過路線發送
    //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;
//將消息的child_frame_id設置爲base_link座標系，因爲這是我們發送速度信息的座標系
    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    r.sleep();
  }
}

1.3發佈傳感器流數據替代PointStamped

通過ROS從傳感器正確發佈數據對於導航堆棧安全運行非常重要。如果導航堆棧沒有收到來自機器人傳感器的信息，那麼它將被盲目地駕駛，並且很有可能會觸發事件。有許多傳感器可用於嚮導航堆棧提供信息：激光器，相機，聲納，紅外線，碰撞傳感器等。但是，當前的導航堆棧只接受使用sensor_msgs / LaserScan消息類型或sensor_msgs / PointCloud消息類型。以下將提供兩種類型消息的典型設置和使用示例。

ROS的消息頭

與衆多通過ROS發佈的消息一樣
sensor_msgs/LaserScan和sensor_msgs/PointCloud消息類型包含tf的座標系和時間相關的信息，爲了使這些發送的信息標準化，這小消息中的消息頭都是必不可少的一部分。

下面列出三種類型的消息頭，當消息從給定發佈者發出時，seq域自動增加消息的識別符。stamp域存儲與消息中的數據有關的時間信息。例如，在激光掃描儀中，stamp對於掃描開始的時間。frame_id域存儲了消息中的數據有關的tf座標系信息。

uint32 seq
time stamp
string frame_id

發送激光雷達(LaserScans)信息

##LaserScans 消息
Header header
float32 angle_min
float32 angle_max
float32 angle_increment

float32 time_increment
float32 scan_time
float32 range_min
float32 range_min
float32[] ranges
float32[] intensities

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "laser_scan_publisher");

  ros::NodeHandle n;
  ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);

  unsigned int num_readings = 100;
  double laser_frequency = 40;
  double ranges[num_readings];
  double intensities[num_readings];

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    //generate some fake data for our laser scan
    for(unsigned int i = 0; i < num_readings; ++i){
      ranges[i] = count;
      intensities[i] = 100 + count;
    }
    ros::Time scan_time = ros::Time::now();

    //populate the LaserScan message
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    scan.angle_min = -1.57;
    scan.angle_max = 1.57;
    scan.angle_increment = 3.14 / num_readings;
    scan.time_increment = (1 / laser_frequency) / (num_readings);
    scan.range_min = 0.0;
    scan.range_max = 100.0;

    scan.ranges.resize(num_readings);
    scan.intensities.resize(num_readings);
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = ranges[i];
      scan.intensities[i] = intensities[i];
    }

    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

發送點雲(PointClouds)信息

#PointClouds 消息
Header header
geometry_msgs/Point32[] points

ChannelFloat32[] channels

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");

  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

  unsigned int num_points = 100;

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";

    cloud.points.resize(num_points);

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 1 + count;
      cloud.points[i].y = 2 + count;
      cloud.points[i].z = 3 + count;
      cloud.channels[0].values[i] = 100 + count;
    }

    cloud_pub.publish(cloud);
    ++count;
    r.sleep();
  }
}

2. SLAM

SLAM也稱爲CML（concurrent mapping and localization），即時定位與地圖構建，或併發建圖與定位。SLAM問題可以描述爲：機器人在未知環境中從一個未知位置開始移動，在移動過程中根據位置估計和地圖進行自身定位，同時在自身定位的基礎上建造增量式地圖，實現機器人的自主定位與導航。

2.1 slam_gmapping功能包

    本功能包包含了來自OpenSlam的ROS軟件包GMapping。GMapping功能包提供了基於激光的SLAM，建立了名爲slam_gampping的節點。使用slam_gmapping，用戶可以根據從激光傳感器輸出的數據創建2D柵格地圖。GMapping是一個從激光傳感器數據建立地圖的非常有效的Rao-Blackwellized粒子濾波軟件包。GMapping文檔：https://svn.openslam.rog/data/svn/gmapping。

    該方法使用粒子濾波方法，其中每個粒子承載了一個獨立的環境地圖。其中，關鍵的問題在於怎樣降低粒子數量。在該軟件包中，採用自適應技術來降低粒子數量。其中採用的方法爲不光考慮移動而且考慮最近的觀測值來計算精確的粒子分佈。這樣，在濾波的預測步，極大降低了機器人位姿的不確定性。更進一步，算法中使用有選擇地重採樣技術，有效降低了粒子耗散對算法的影響。

    使用：機器人平臺要提供里程數據並且裝備了水平安裝的固定的激光測距儀。slam_gmapping節點將把收到的數據進行tf變換。

爲了讓機器人建立地圖，需要在base_scan主題發佈掃描數據：

rosrun gmapping slam_gmapping scan:=base_scan

節點

1)slam_gmapping
    slam_gmapping節點在sensor_msgs/LaserScan消息內獲取數據並建立地圖(nav_msgs/OccupancyGrid)。該地圖可以通過ROS主題或服務來獲取。

2)訂閱的topic
    (1)tf(tf/tfMessage)：與座標系相關的變換。
    (2)sacn(sensor_msgs/LaserScan):激光掃描數據

3)發佈的topic
    (1)map_metadata(nav_msgs/MapMetaData)：從這個週期性更新的主題獲取地圖數據。
    (2)map(nav_msgs/OccupancyGrid):從這個週期性更新的主題獲取地圖。
    (3)entropy(std_msgs/Float64):估計機器人位姿分佈的熵。

4)服務
    dynamic_map(nav_msgs/GetMap):調用該服務以獲取地圖數據。

5)參數
     P165

6)需要的tf變換
    (1)<the frame attached to incoming scans> -> base_link:通常是一個固定值，通過robot_state_publisher或tf的static_transform_publisher進行週期性廣播。
    (2)base_link -> odom:通常由里程計系統提供。

7)提供的tf變換
    map -> odom:當前地圖座標系中機器人位姿的估計。

使用記錄的數據建立地圖

1.建立地圖

1）如果已經安裝源文件，首先編譯GMapping
2）獲取一個bag：自己創建或者從下載測試包
3）運行ROS core
4）設置參數：rosparam set use_sim_time true
5）運行GMapping：rosrun gmapping slam_gmapping scan:=base_scan

在新終端中，回放消息記錄包，反饋數據給slam_gmapping：

rosbag play <name of the bag that you downloaded / created in step 2>

運行之後，等程序執行完成，退出。

最終，可以保存建立的地圖爲pgm文件：rosrun map_server map_saver。

2.下載測試包

wget http://pr.willowgarage.com/data/gmapping/basic_localization_stage.bag

3.建立個人程序包

1.搭建自己的機器人，裝備激光傳感器，發佈變換數據，並可以操作移動。
2.記錄掃描與變化數據：
rosbag record -O mylaserdata /base_scan /tf
#在當前目錄寫入一個名爲mylaserdata_<DATE>-topic.bag文件
3.驅動機器人移動
4.結束rosbag

4.實時查看建立地圖的過程

如果想實時的查看地圖，而不是等所有進程結束之後，可執行下命令：

1.編譯nav_view : rosmake nav_view 
2.重複上一步，直至有日誌未見可以回放數據，然後運行slam_gmapping
3.運行nav_view : rosrun nav_view nav_view /static_map:=/dynamic_map。隨後單擊重載地圖按鈕，即可看到最新地圖。

5.模擬器中建立地圖

sudo apt-get install ros-diamondback-pr2all

svn co https://code.ros.org/svn/wg-ros-pkg/stacks/wg_robots_gazebo/trunk/pr2_build_map_gazebo_demo/

#將上一步的路徑加到ROS_PACKAGE_PATH然後編譯 
rosmake pr2_build_map_gazebo_demo

#運行
roslaunch pr2_build_map_gazebo_demo pr2_build_map_gazebo_demo.launch

#在rviz窗口左側面板進行設置：
Global Options中固定座標系(Fixed Frame)設置爲 /omod_combined
Global Options中目標座標系(Target Frame)設置爲 /odom_combined

#在rviz中看到的只是地圖的一部分，爲了建立完整地圖，需要讓機器人運動
#在新的終端運行
roslaunch pr2_teleop teleop_keyboard.launch 
rosrun map_server map_saver -f map ##保存地圖

6.模擬器中使用客戶定製地圖

首先需要一個地圖文件map.pgm，然後把它放在pr2_gazebo功能包下。

編譯 rosmake pr2_2dnav_gazebo

創建啓動文件 my_map_sim.launch

啓動程序
export ROBOT = sim
roslaunch my_map_sim.launch

<!-- my_map_sim.launch -->
<launch>
<include file="$(find gazebo_worlds)/launch/office_world.launch"/>
<include file="$(find pr2_gazebo)/pr2.launch"/>

<node pkg="map_server" type="map_server" args="$(find pr2_gazebo)/map.pgm 0.05" respawn="true" name="map1" />

<node pkg="pr2_tuckarm" type="tuck_arms.py" args="b" output="screen" name="tuck_arms" />

<include file="$(find pr2_2dnav_gazebo)/2dnav-stack-amcl.launch" />
<include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch" />

</launch>

10.SLAM和導航

1. 爲機器人配置TF

1.1 變換配置

1.2發佈里程信息

1.3發佈傳感器流數據替代PointStamped

2. SLAM

2.1 slam_gmapping功能包

前端使用 Konva 實現可視化設計器（13）- 折線 - 最優路徑應用【思路篇】

Linux系統編程3.時間概念

Linux系統編程4.信號處理

Linux系統編程6.線程操作

Linux系統編程5.消息管理

仿printf實現

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結