Method Init

roborts_localization/localization_node.cpp:28–74

bool LocalizationNode::Init() {

  LocalizationConfig localization_config;
  localization_config.GetParam(&nh_);

  odom_frame_   = std::move(localization_config.odom_frame_id);
  global_frame_ = std::move(localization_config.global_frame_id);
  base_frame_   = std::move(localization_config.base_frame_id);

  laser_topic_ = std::move(localization_config.laser_topic_name);

  init_pose_ = {localization_config.initial_pose_x,
                localization_config.initial_pose_y,
                localization_config.initial_pose_a};
  init_cov_ = {localization_config.initial_cov_xx,
               localization_config.initial_cov_yy,
               localization_config.initial_cov_aa};

  transform_tolerance_  = ros::Duration(localization_config.transform_tolerance);
  publish_visualize_ = localization_config.publish_visualize;

  tf_broadcaster_ptr_ = std::make_unique<tf::TransformBroadcaster>();
  tf_listener_ptr_ = std::make_unique<tf::TransformListener>();

  distance_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("distance_map", 1, true);
  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);

  // Use message filter for time synchronizer (laser scan topic and tf between odom and base frame)
  laser_scan_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::LaserScan>>(nh_, laser_topic_, 100);
  laser_scan_filter_ = std::make_unique<tf::MessageFilter<sensor_msgs::LaserScan>>(*laser_scan_sub_,
                                                                                   *tf_listener_ptr_,
                                                                                   odom_frame_,
                                                                                   100);
  laser_scan_filter_->registerCallback(boost::bind(&LocalizationNode::LaserScanCallback, this, _1));

  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &LocalizationNode::InitialPoseCallback, this);
  pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("amcl_pose", 2, true);

  amcl_ptr_= std::make_unique<Amcl>();
  amcl_ptr_->GetParamFromRos(&nh_);
  amcl_ptr_->Init(init_pose_, init_cov_);

  map_init_ = GetStaticMap();
  laser_init_ = GetLaserPose();

  return map_init_&&laser_init_;
}

bool LocalizationNode::GetStaticMap(){
  static_map_srv_ = nh_.serviceClient<nav_msgs::GetMap>("static_map");