amcl源码解读

最新推荐文章于 2024-05-29 20:22:33 发布

loliman

最新推荐文章于 2024-05-29 20:22:33 发布

阅读量1.7k

点赞数 4

文章标签： ros amcl

本文链接：https://blog.csdn.net/qq_33651375/article/details/98170799

版权

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "amcl");
  ros::NodeHandle nh;
  signal(SIGINT, sigintHandler);
  amcl_node_ptr.reset(new AmclNode());
  if (argc == 1)
  {
    ros::spin();
  }
  else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
  {
    amcl_node_ptr->runFromBag(argv[2]);
  }
  amcl_node_ptr.reset();
  return(0);
}

首先是main函数，第一步常规操作初始化了amcl的ros节点，并实例化。第二步使用signal函数：第一个参数为信号功能参数，第二个参数为自定义函数，选用SIGINT表示信号中断即用户输入ctrl+c，之后信号被捕获，进入自定义的信号处理函数中

void sigintHandler(int sig)
{
  // Save latest pose as we're shutting down.
  amcl_node_ptr->savePoseToServer();
  ros::shutdown();
}

这个函数中使用到了智能指针，他的声明在这

boost::shared_ptr<AmclNode> amcl_node_ptr;

boost::shared_ptr是智能指针;<AmclNode>是类模板;amcl_node_ptr为指针对象。回到main函数中，看着这句代码

amcl_node_ptr.reset(new AmclNode());

shared_ptr可以通过reset方法初始化对象，此时利用AmclNode类构造函数对指针进行初始化。首先给对象分配内存，地址为x，然后调用类构造函数y，将类构造函数的地址y写入x中。接下来就是amcl的构造函数：

AmclNode::AmclNode()
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  double tmp;
  std::string tmp_model_type;
  transform_tolerance_.fromSec(tmp_tol);
  {
    double bag_scan_period;
    private_nh_.param("bag_scan_period", bag_scan_period, -1.0);
    bag_scan_period_.fromSec(bag_scan_period);
  }
  updatePoseFromServer();
  cloud_pub_interval.fromSec(1.0);
  tfb_.reset(new tf2_ros::TransformBroadcaster());
  tf_.reset(new tf2_ros::Buffer());
  tfl_.reset(new tf2_ros::TransformListener(*tf_));

  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
  global_loc_srv_ = nh_.advertiseService("global_localization", 
					 &AmclNode::globalLocalizationCallback,
                                         this);
  nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);
  set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);

  laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
  laser_scan_filter_ = 
          new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                                                             *tf_,
                                                             odom_frame_id_,
                                                             100,
                                                             nh_);
  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                   this, _1));
  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

  if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
    requestMap();
  }
  m_force_update = false;

  dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
  dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);

  // 15s timer to warn on lack of receipt of laser scans, #5209
  laser_check_interval_ = ros::Duration(15.0);
  check_laser_timer_ = nh_.createTimer(laser_check_interval_, 
                                       boost::bind(&AmclNode::checkLaserReceived, this, _1));

  diagnosic_updater_.setHardwareID("None");
  diagnosic_updater_.add("Standard deviation", this, &AmclNode::standardDeviationDiagnostics);
}

amcl的构造函数，一大部分用于装载参数，这部分由于篇幅原因被我无情删除。解读剩下的部分：首先是这句代码定义递归互斥量

boost::recursive_mutex::scoped_lock l(configuration_mutex_);

这是boost库中的线程区域锁，锁的初始化放在amcl构造函数中，锁的释放放在析构函数中，为了防止多线程访问共享资源而产生的对数据篡改。

updatePoseFromServer();

装载初始位姿。接下来三行是智能指针，并且三个对象分别是发布tf的指针，接收tf的指针指向buffer的指针，都进行构造函数的初始化。

  std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
  std::shared_ptr<tf2_ros::TransformListener> tfl_;
  std::shared_ptr<tf2_ros::Buffer> tf_;  
  tfb_.reset(new tf2_ros::TransformBroadcaster());
  tf_.reset(new tf2_ros::Buffer());
  tfl_.reset(new tf2_ros::TransformListener(*tf_));

之后的部分则是，定义发布amcl_pose、particlecloud话题，定义gloabl_localization、request_nomotion_update、set_map服务端，定义话题订阅端，订阅initialpose、map、scan。其中有这么一句消息过滤器，作用是同步tf和scan信号:

  laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
  laser_scan_filter_ = 
          new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                                                             *tf_,
                                                             odom_frame_id_,
                                                             100,
                                                             nh_);
  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                   this, _1));

具体函数功能在http://docs.ros.org/api/tf2_ros/html/c++/classtf2__ros_1_1MessageFilter.html#aaad9d03a10d89188c0270e5440d19713这里。下面的代码是订阅map，默认的use_map_topic是false，所以默认调用的是requetMap函数。

  if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
    requestMap();
  }

来到requestMap函数：首先依旧是线程锁，之后是一个while函数阻塞等待地图，等到地图后跳出while循环，执行handleMapMessage()函数，地图信息装载在resp对象中。

void AmclNode::requestMap()
{
  boost::recursive_mutex::scoped_lock ml(configuration_mutex_);

  // get map via RPC
  nav_msgs::GetMap::Request  req;
  nav_msgs::GetMap::Response resp;
  ROS_INFO("Requesting the map...");
  while(!ros::service::call("static_map", req, resp))
  {
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
  }
  handleMapMessage( resp.map );
}

接下来是handleMapMessage函数中：下面的函数用于清空map_,pf_,odom_,laser_。

std::vector< AMCLLaser* > lasers_;
std::vector< bool > lasers_update_;
std::map< std::string, int > frame_to_laser_;

freeMapDependentMemory();
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();

清空完所有信息，保存接收到的地图信息，并进行填充：

map_ = convertMap(msg);

进入convertMap函数中：这里导入了map.yaml文件里的参数，并把地图的数据转为地图每个点的状态都抱存在map->cell[].occ_state中（map_是个指针，指向的cell也是指针，通过数组化转换为内容，并对数组内的成员进行赋值。

map_t* AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{
  map_t* map = map_alloc();
  ROS_ASSERT(map);

  map->size_x = map_msg.info.width;
  map->size_y = map_msg.info.height;
  map->scale = map_msg.info.resolution;
  map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
  map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
  // Convert to player format
  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
  ROS_ASSERT(map->cells);
  for(int i=0;i<map->size_x * map->size_y;i++)
  {
    if(map_msg.data[i] == 0)
      map->cells[i].occ_state = -1;
    else if(map_msg.data[i] == 100)
      map->cells[i].occ_state = +1;
    else
      map->cells[i].occ_state = 0;
  }
  return map;
}

填充完地图，开始均匀采样：

#if NEW_UNIFORM_SAMPLING
  // Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));
#endif

free_space_indices是一个线性容器，利用resize操作把容器的长度删为0。然后遍历每个状态为-1的像素点，如果有则往容器的末尾添加由std:make_pair所构建的包含i与j位置坐标的结构体。继续往下看，开始创建粒子滤波器：

  // Create the particle filter
  pf_ = pf_alloc(min_particles_, max_particles_,
                 alpha_slow_, alpha_fast_,
                 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                 (void *)map_);
  pf_->pop_err = pf_err_;
  pf_->pop_z = pf_z_;
  // Initialize the filter
  updatePoseFromServer();
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = init_pose_[0];
  pf_init_pose_mean.v[1] = init_pose_[1];
  pf_init_pose_mean.v[2] = init_pose_[2];
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  pf_init_pose_cov.m[0][0] = init_cov_[0];
  pf_init_pose_cov.m[1][1] = init_cov_[1];
  pf_init_pose_cov.m[2][2] = init_cov_[2];
  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
  pf_init_ = false;

来看到pf_alloc函数：

pf_t *pf_alloc(int min_samples, int max_samples,
               double alpha_slow, double alpha_fast,
               pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
  int i, j;
  pf_t *pf;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  
  srand48(time(NULL));

  pf = calloc(1, sizeof(pf_t));

  pf->random_pose_fn = random_pose_fn;
  pf->random_pose_data = random_pose_data;

  pf->min_samples = min_samples;
  pf->max_samples = max_samples;

  pf->pop_err = 0.01;
  pf->pop_z = 3;
  pf->dist_threshold = 0.5;  
  pf->current_set = 0;
  for (j = 0; j < 2; j++)
  {
    set = pf->sets + j;     
    set->sample_count = max_samples;
    set->samples = calloc(max_samples, sizeof(pf_sample_t));
    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->pose.v[0] = 0.0;
      sample->pose.v[1] = 0.0;
      sample->pose.v[2] = 0.0;
      sample->weight = 1.0 / max_samples;
    }
    // HACK: is 3 times max_samples enough?
    set->kdtree = pf_kdtree_alloc(3 * max_samples);
    set->cluster_count = 0;
    set->cluster_max_count = max_samples;
    set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
    set->mean = pf_vector_zero();
    set->cov = pf_matrix_zero();
  }
  pf->w_slow = 0.0;
  pf->w_fast = 0.0;
  pf->alpha_slow = alpha_slow;
  pf->alpha_fast = alpha_fast;
  //set converged to 0
  pf_init_converged(pf);
  return pf;
}

输入的参数有min_samples,max_samples,alpha_slow,slpha_fast,random_pose_fn,random_pose_data,一开始先创造了一个pf来存关于粒子的参数，然后给每个粒子分配初始权重和方向。并从yaml中装载滤波器的均值和方差。然后初始化了里程计模型和雷达模型。

  // Odometry
  delete odom_;
  odom_ = new AMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new AMCLLaser(max_beams_, map_);
  ROS_ASSERT(laser_);

首先看AMCLOdom类的构造函数

回到构造函数，这三行代码是用来装载参数服务器的

dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);

最后创建了一个定时器来定时运行AmclNode::checkLaserReceived这个函数，使用了boost::bind

check_laser_timer_ = nh_.createTimer(laser_check_interval_, 
                                       boost::bind(&AmclNode::checkLaserReceived, this, _1));

main函数到此结束，开始分析回调函数。

首先是最主要的回调函数laserReceived

laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));

AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
  std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
  last_laser_received_ts_ = ros::Time::now();
  if( map_ == NULL ) {
    return;
  }
  boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
  int laser_index = -1;

  if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())
  {
    ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str());
    lasers_.push_back(new AMCLLaser(*laser_));
    lasers_update_.push_back(true);
    laser_index = frame_to_laser_.size();

    geometry_msgs::PoseStamped ident;
    ident.header.frame_id = laser_scan_frame_id;
    ident.header.stamp = ros::Time();
    tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);

    geometry_msgs::PoseStamped laser_pose;
    try
    {
      this->tf_->transform(ident, laser_pose, base_frame_id_);
    }
    catch(tf2::TransformException& e)
    {
      ROS_ERROR("Couldn't transform from %s to %s, "
                "even though the message notifier is in use",
                laser_scan_frame_id.c_str(),
                base_frame_id_.c_str());
      return;
    }

    pf_vector_t laser_pose_v;
    laser_pose_v.v[0] = laser_pose.pose.position.x;
    laser_pose_v.v[1] = laser_pose.pose.position.y;

    laser_pose_v.v[2] = 0;
    lasers_[laser_index]->SetLaserPose(laser_pose_v);
    ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
              laser_pose_v.v[0],
              laser_pose_v.v[1],
              laser_pose_v.v[2]);

    frame_to_laser_[laser_scan_frame_id] = laser_index;
  } else {
    laser_index = frame_to_laser_[laser_scan_frame_id];
  }

  pf_vector_t pose;
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }


  pf_vector_t delta = pf_vector_zero();

  if(pf_init_)
  {
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }
  bool force_publication = false;
  if(!pf_init_)
  {
    pf_odom_pose_ = pose;
    pf_init_ = true;
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;
    force_publication = true;
    resample_count_ = 0;
  }
  else if(pf_init_ && lasers_update_[laser_index])
  {
    AMCLOdomData odata;
    odata.pose = pose;
    odata.delta = delta;
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
  }

函数的一开始就将雷达坐标系进行赋值，如果没有map的信息也不执行此程序。

std::map< std::string, int > frame_to_laser_;
if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())

在map中查找key=laser_scan_frame_id的数据，如果查不到就往下执行。

amcl构造函数结束。

loliman

关注

4
点赞
踩
20

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫