Ros學習——movebase源碼解讀之amcl

1.amcl的cmakelists.txt文件

add_executable(amcl  src/amcl_node.cpp)

target_link_libraries(amcl

 amcl_sensors amcl_map amcl_pf
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES}
)

該項目生成一個amcl節點；以及amcl_sensors amcl_map amcl_pf三個庫

2.amcl node

2.1 類結構

class amcl_node
{
public:
    amcl_node();
    ~amcl_node();
    void runFromBag(const std::string &in_bag_fn);//根據信息記錄包來運行amcl

    int process();
    void savePoseToServer();////把位姿信息保存到參數服務器

private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
    std::shared_ptr<tf2_ros::TransformListener> tfl_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
    bool sent_first_transform_;
    tf2::Transform latest_tf_;
    bool latest_tf_valid_;

    static pf_vector_t uniformPoseGenerator(void* arg);

    static std::vector<std::pair<int, int> > free_space_indices;
    // Callbacks
    bool globalLocalizationCallback(std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res);
    bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res);
    bool setMapCallback(nav_msgs::SetMap::Request& req,
        nav_msgs::SetMap::Response& res);

    void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
    void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
    void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
    void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);

    void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
    void freeMapDependentMemory();
    map_t* convertMap(const nav_msgs::OccupancyGrid& map_msg);
    void updatePoseFromServer();
    void applyInitialPose();

    //parameter for what odom to use
    std::string odom_frame_id_;

    //paramater to store latest odom pose
    geometry_msgs::PoseStamped latest_odom_pose_;

    //parameter for what base to use
    std::string base_frame_id_;
    std::string global_frame_id_;

    bool use_map_topic_;
    bool first_map_only_;

    ros::Duration gui_publish_period;
    ros::Time save_pose_last_time;
    ros::Duration save_pose_period;

    geometry_msgs::PoseWithCovarianceStamped last_published_pose;

    map_t* map_;
    char* mapdata;
    int sx, sy;
    double resolution;

    message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
    tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
    ros::Subscriber initial_pose_sub_;
    std::vector< AMCLLaser* > lasers_;
    std::vector< bool > lasers_update_;
    std::map< std::string, int > frame_to_laser_;

    // Particle filter
    pf_t *pf_;
    double pf_err_, pf_z_;
    bool pf_init_;
    pf_vector_t pf_odom_pose_;
    double d_thresh_, a_thresh_;
    int resample_interval_;
    int resample_count_;
    double laser_min_range_;
    double laser_max_range_;

    //Nomotion update control
    bool m_force_update;  // used to temporarily let amcl update samples even when no motion occurs...

    AMCLOdom* odom_;
    AMCLLaser* laser_;

    ros::Duration cloud_pub_interval;
    ros::Time last_cloud_pub_time;

    // For slowing play-back when reading directly from a bag file
    ros::WallDuration bag_scan_period_;

    void requestMap();//請求服務static_server提供map，然后調用handleMapMessage處理地圖信息

    // Helper to get odometric pose from transform system
    bool getOdomPose(geometry_msgs::PoseStamped& pose,
        double& x, double& y, double& yaw,
        const ros::Time& t, const std::string& f);

    //time for tolerance on the published transform,
    //basically defines how long a map->odom transform is good for
    ros::Duration transform_tolerance_;

    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    ros::Publisher pose_pub_;
    ros::Publisher particlecloud_pub_;
    ros::ServiceServer global_loc_srv_;
    ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
    ros::ServiceServer set_map_srv_;
    ros::Subscriber initial_pose_sub_old_;
    ros::Subscriber map_sub_;

    amcl_hyp_t* initial_pose_hyp_;
    bool first_map_received_;
    bool first_reconfigure_call_;

    boost::recursive_mutex configuration_mutex_;
    dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_;
    amcl::AMCLConfig default_config_;
    ros::Timer check_laser_timer_;

    int max_beams_, min_particles_, max_particles_;
    double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
    double alpha_slow_, alpha_fast_;
    double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
    //beam skip related params
    bool do_beamskip_;
    double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
    double laser_likelihood_max_dist_;
    odom_model_t odom_model_type_;
    double init_pose_[3];
    double init_cov_[3];
    laser_model_t laser_model_type_;
    bool tf_broadcast_;

    void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);

    ros::Time last_laser_received_ts_;
    ros::Duration laser_check_interval_;
    void checkLaserReceived(const ros::TimerEvent& event);
};

 

 

2.2 main函數

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

  // Override default sigint handler
  signal(SIGINT, sigintHandler);

  // Make our node available to sigintHandler
  amcl_node_ptr.reset(new AmclNode());

  if (argc == 1)
  {
    // run using ROS input
    ros::spin();
  }
  else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
  {
    amcl_node_ptr->runFromBag(argv[2]);
  }

  // Without this, our boost locks are not shut down nicely
  amcl_node_ptr.reset();

  // To quote Morgan, Hooray!
  return(0);
}

 

2.3 關鍵步驟

0.構造函數AmclNode()

——>參數配置：粒子濾波參數,運動模型參數,觀測模型參數等

——>updatePoseFromServer()：從參數服務器中獲取初始位姿及初始分布

——>pose和particle息發布:

1. amcl_pose： geometry_msgs::PoseWithCovarianceStamped，后驗位姿+一個6*6的協方差矩陣（xyz+三個轉角）
2. particlecloud：geometry_msgs::PoseArray，粒子位姿的數組

——>創建服務：

1. global_localization：&AmclNode::globalLocalizationCallback，這里是沒有給定初始位姿的情況下在全局范圍內初始化粒子位姿，該Callback調用pf_init_model，然后調用AmclNode::uniformPoseGenerator在地圖的free點隨機生成pf->max_samples個粒子
2. request_nomotion_update：&AmclNode::nomotionUpdateCallback沒運動模型更新的情況下也暫時更新粒子群
3. set_map：&AmclNode::setMapCallback：//handleMapMessage()進行地圖轉換 ，記錄free space ，以及初始化pf_t 結構體,實例化運動模型(odom)和觀測模型(laser)；          //handleInitialPoseMessage(req.initial_pose); 根據接收的初始位姿消息,在該位姿附近高斯采樣重新生成粒子集
4. dynamic_reconfigure::Server動態參數配置器。

——>訂閱話題：

1. scan_topic_：sensor_msgs::LaserScan，AmclNode::laserReceived()：回調函數laserReceived是粒子濾波主要過程,根據激光掃描數據更新粒子
2. initialpose：AmclNode::initialPoseReceived()：這個應該就是訂閱rviz中給的初始化位姿，調用AmclNode::handleInitialPoseMessage，只接受global_frame_id_（一般為map）的坐標，並重新生成粒子。在接收到的初始位姿附近采樣生成 粒子集。
3. map：AmclNode::mapReceived這個在use_map_topic_的時候才訂閱，否則requestMap();我這里也沒有訂閱，因為只使用了一個固定的地圖。

——>一個15秒的定時器：AmclNode::checkLaserReceived，檢查 15上一次收到激光雷達數據至今是否超過15秒，如超過則報錯。

 

1.requestmap()

——>requestMap：一直請求服務static_map直到成功

——>handleMapMessage()：  1.將受到的msg轉換成標准地圖，0->-1(不是障礙);100->+1（障礙）;else->0（不明）

　　　　　　　　　　　　　　2.提取非障礙部分，列入Vector類型的free_space_indices

　　　　　　　　　　　　　　3.創建粒子濾波器——>updatePoseFromServer()——>初始化粒子濾波器——>初始化傳感器（odom，laser）——>applyInitialPose()

 

2.laserReceived()

 

——>獲取laser對應於baselink的坐標

——>獲取baselink對應於odom的坐標
——>根據里程計的變化值+高斯噪音 更新 pf_t中samples的內里程計值（運動模型）
odom->updateAction()
——>根據當前雷達數據更新各里程計對應的權值weights
laser_[laser_index]->updateSensor（）
——>得到濾波結果后,分別在話題/amcl_pose和/ particlecloud上發布位姿和粒子集

 

3.主要過程

• 構造時初始化,從參數服務器中獲取數據初始化各類參數;(接收地圖設置,gui顯示發布頻率,保存位姿到參數服務器頻率,laser測距范圍及其概率模型參數,odom概率模型參數,粒子濾波及kld重采樣參數,從參數服務器獲取初始位姿,然后初始化了訂閱者,發布者,服務)
• 地圖加載,兩種方式(1.訂閱/map話題2.請求服務得到地圖),得到地圖后也有個初始化過程(將消息類型的地圖轉換為定義的map類數據,統計free狀態的柵格索引,從參數服務器獲取位姿信息,並初始化粒子濾波器pf_,初始化odom模型參數,初始化laser模型參數)
• 粒子濾波,訂閱laser_scan的回調函數中處理,得到結果后發布位姿和粒子集
• initialpose的回調,接收到初始位姿消息后,融入最新的里程改變,然后在該位姿附近重新生成粒子集

4.主要數據類型與算法

4.1 pf

 

1. eig3.c

實現的是一個3x3對稱矩陣的特征值與特征向量的計算,首先用Householder矩陣將矩陣變換為三對角矩陣,然后使用ql分解迭代計算 。

2. pf_kdtree.c定義了一個kdtree以及維護方法來管理所有粒子 ：創建、銷毀、清除元素、插入元素、計算概率估計、比較、查找、

typedef struct
{
  // Cell size
  double size[3];

  // The root node of the tree
  pf_kdtree_node_t *root;

  // The number of nodes in the tree
  int node_count, node_max_count;
  pf_kdtree_node_t *nodes;

  // The number of leaf nodes in the tree
  int leaf_count;

} pf_kdtree_t;

 

3.pf_pdf.c主要定義了一個從給定pdf中采樣粒子的方法

4.pf_vector.c定義了三維列向量和三維矩陣和基本的運算方法：加、減、全局和局部坐標系變換、是否NAN或INF

5.pf.c定義了粒子單元pf_sample_t,粒子集pf_sample_set_t,粒子濾波pf_t的數據類型,還有一個 pf_cluster_t表示粒子集的聚類信息,關鍵函數主要包含如下三個,分別對應粒子濾波中的運動更新,觀測更新,重采樣三個過程

 

 

4.2 sensors

1. amcl_sencor.cpp

——>定義了基類，以虛函數InitSensor()、UpdateSensor()、UpdateAction()提供接口

 

2. amcl_laser.cpp

——>定義了激光數據類型,三種觀測更新模型(詳細見<<概率機器人>>),具體實現了UpdateSensor,用於計算粒子權值

 

3. amcl_odom.cpp
——>具體實現了基類定義的UpdateAction函數,用於根據運動更新粒子,定義了兩種運動模型,差分和全向

 

4.3 map

——>map中主要定義了概率柵格地圖的數據表示

typedef struct
{  
int occ_state;// Occupancy state (-1 = free, 0 = unknown, +1 = occ) 
double occ_dist;// Distance to the nearest occupied cell
} map_cell_t;

// Description for a map
typedef struct
{
  // Map origin; the map is a viewport onto a conceptual larger map.
  double origin_x, origin_y;
  
  // Map scale (m/cell)
  double scale;

  // Map dimensions (number of cells)
  int size_x, size_y;
  
  // The map data, stored as a grid
  map_cell_t *cells;

  // Max distance at which we care about obstacles, for constructing
  // likelihood field
  double max_occ_dist;
  
} map_t;

 

 

部分參考：https://blog.csdn.net/qq_27753669/article/details/80011156